尝试关于单目视觉 SLAM 的空间感知定位技术的学习，做以调查。SLAM算法最早在机器人领域中提出，视觉SLAM又可以分为单目、双目和深度相机三种传感器模式，在AR应用中通常使用轻便、价格低廉的单目相机设备。仅使用一个摄像头作为传感器完成同步定位与地图创建操作，具有系统结构简单、成本低且易实现等优点。

正是尺度不确定性才使得单目SLAM 能够在大尺度和小尺度环境之间进行自由转换。所以单目视觉SLAM成为增强现实领域的研究热点。我近期的工作将从以下几个方面进行阐述：PTAM算法、ORB-SLAM2算法、ORB-SLAM3算法。

一、PTAM算法

1、PTAM算法复现

PTAM算法是2007年提出的经典的单目特征点法SLAM，同时也是早期将SLAM和AR结合起来的工作之一。虽然PTAM几乎已经过时，但其在整个SLAM发展过程中占有重要地位。

首先我跑了经典算法PTAM，全称Parallel Tracking And Mapping，是最早提出将Track和Map分开作为两个线程的一种SLAM算法，是一种基于关键帧的单目视觉SLAM算法。
David等人提出PTAM,第一个使用非线性优化的方法也是第一个将BA与视觉SLAM相结合的方法,首次出现前后端的概念，为后来视觉SLAM提供了良好思路。

PTAM是视觉SLAM领域里程碑式的项目。在此之前，MonoSLAM为代表的基于卡尔曼滤波的算法架构是主流，它用单个线程逐帧更新相机位置姿态和地图。地图更新的计算复杂度很高，为了做到实时处理（30Hz），MonoSLAM每帧图片只能用滤波的方法处理约10~12个最稳定的特征点。PTAM最大的贡献是提出了tracking、mapping双线程的架构，tracking线程只需要逐帧更新相机位置姿态，可以很容易实现实时计算；而mapping线程并不需要逐帧更新，有更长的处理时间，原来只能用在离线SfM（Structure from Motion）的BA（Bundle Adjustment）也可以用起来。这种基于优化的算法比滤波法在单位计算时间可以得到更高的精度。这种多线程的处理方式也更顺应现代CPU的发展趋势。之后的视觉SLAM算法几乎全部沿用了这一思想。

(1)配置PTAM算法

使用PTAM算法需要①安装ROS kinetic②编译PTAM③获取PTAM并编译④标定相机
相机标定已有很多开源的工具，比如Matlab、OpenCV和ROS都提供了一些工具包。这里使用PTAM自带的工具进行标定。首先在一个新的终端打开roscore。然后启动相机并发布消息，不同的相机需要打开不同的launch文件。

(2)转化灰度图

因为PTAM标定要求输入为灰度图像，必须首先将彩色的image_raw转换为灰度图，否则会出现issues#78 这样的图像重影模糊问题。因此使用的自带的image_proc节点来转换：
这时再查看rostopic list会发现多了很多topic，其中camera/image_mono表示灰度图。
然后需要根据相机的情况，对~/catkin_ws/src/ethzasl_ptam/ptam/PtamFixParams.yaml更改相关的配置参数:

(3)标定相机

接着运行标定节点，记得将变量名重新映射(remap)一下即可：
至此就可以开始标定相机了，标定后将保存文件camera.cfg。

(3)运行PTAM

2、PTAM 算法总结

它既可以提供实时的定位和建图，也可以在虚拟平面上叠加虚拟物体，PTAM算是早期的结合AR 的SLAM 工作之一。与许多早期工作相似，存在着明显的缺陷：场景小，跟踪容易丢失，等等。这些又在后续的方案中得以修正。

二、ORB-SLAM2算法

到目前为止,ORB-SLAM 是最完整的基于特征点法 VSLAM, 它可以看做是 PTAM 的一个延伸。2016年，Raul 等人提出了ORB-SLAM 算法，它是一个开源的 SLAM 算法，该算法支持单目、双目和 RGB-D 相机，在大多数环境中均可以运行。该系统在环境场景变化较快以及移动平台高速移动的情况下也有很好的鲁棒性。ORB-SLAM 系统采用的是基于特征点方法对相机的轨线进行实时计算，并根据提取的特征点和计算出的轨迹生成周围环境的三维稀疏特征点地图为了增加ORB-SLAM算法的应用性，作者在其基础上提出了支持双目相机和RGB-D相机的ORB-SLAM2系统。为实现ORB-SLAM2需要实现一些数学库以及理解原理。

1、Pangolin库

可视化的库，SLAM的可视化绘图库，基于OpenGL的，主要优势在于使用比OpenGL简单

2、OpenCV(C++版)

开源的计算机视觉编程程序库，强大的计算机视觉库，相机标定和三维重建算法，如SfM和PnP等。

3、Eigen

适用范围广，支持包括固定大小、任意大小的所有矩阵操作，甚至是稀疏矩阵；支持所有标准的数值类型，并且可以扩展为自定义的数值类型；支持多种矩阵分解及其几何特征的求解；它不支持的模块生态系统提供了许多专门的功能，如非线性优化，矩阵功能，多项式解算器，快速傅立叶变换等。

4、点云库PCL(Point Cloud Library)

实现了大量点云相关的通用算法和高效的数据管理。

5、非线性优化库G2O

在SLAM领域就是预测值或者说是基于模型的预期结果和测量之间的误差，使用G2O解决优化问题。

6、Ceres

谷歌开发C++库Ceres Solver，用来解决带边界约束的非线性最小二乘问题

7、视觉和惯性数据融合

只依赖单目视觉SLAM，感知定位效果仍然不能够满足，ORB-SLAM2可以借助IMU传感器提高提取精度，视觉和惯性数据融合的第一步，是一段松耦合过程，视觉SLAM常见的QR分解SVD分解等矩阵分解方式求解满秩和亏秩最小二乘问题，需要做多模态融合的工作，ORB-SLAM2很多需要修改，不能直接使用。

8、各传感器的时间同步

通过ROS系统对单目相机和IMU发布的话题进行数据交换，IMU初始化会出现问题

三、ORB-SLAM3算法

ORB-SLAM3在2的基础上，引进了IMU的初始化和融合估计，以及子地图功能。
ORB-SLAM3有IMU模式，ORB-SLAM3的回环检测方法相比于2的有所改进，首先确定的一个地方是：即使系统中存在子地图，我们也只会存储唯一的一个BOW数据集，而每个关键帧都会存储各自所在的子地图，以此来加以区分。具体的回环检测策略大体上与ORB-SLAM2并无太大的区别，为了满足实时性的要求需要在ROS下跑ORB-SLAM3。

1、ROS系统

ROS全称Robot Operating System（机器人操作系统）, 集成了大量的工具，库，协议，提供类似OS所提供的功能，SLAM在ROS下跑和非ROS下跑的区别：可以把ROS理解成Linux下集成了许多好用的第三方和多进程编程的一个库集。在ROS下跑SLAM可以使用ROS的许多成熟的方法库。比如进程间通信可以默认使用ROSMaster建立的本地socket通信。不在ROS下跑，很多进程间通信、同步、回调之类的就需要自己去设计。

Ubuntu20.04的ROS环境安装ORB-SLAM3，执行单目实时usb_cam效果，

2、张正友相机标定方法

在ROS中已经集成了对相机完成标定以及畸变矫正的整个流程，用起来非常方便。通常使用张正友相机标定方法，即利用平面坐标的单应性关系，流程简洁，注意标定过程中左右，上下，前后，对角方向前后倾斜标定板，这样使得右边X，Y，Size,Skew保存到相机配置文件中，将标定结果保存至本地。

3、IMU模式

预积分的计算原理主要是根据物理运动公式计算，即加速度，速度和位移之间的计算关系。在ORB-SLAM3中，IMU预积分最早是从第2帧开始的，因为是使用当前帧和上一帧之间的IMU数据，所以只能从第二帧开始。

4、INDMIND的IMU相机

实验室做视觉SLAM需要IMU相机，购入INDEMIND双目惯性模组运行多模态融合的感知定位，根据官方例程加以改进，运行实例并记录。IDEMIND这款双目摄像头可以匹配ubuntu18.04和ubuntu16.04两个版本的ubuntu，但是最好使用ubuntu16.04版本。
双目惯性模组 SDK1.4.2 版本改进了不少，官网上关于ROS环境下的教程十分简略，直接按照官网的步骤无法实现相机数据读取并发布到指定的topic上，在源码上进行修改，重新建构一个ROS节点发布话题。

修改ORB-SLAM3源码的跟踪线程，当前配置IMU相机使用yaml把相机外参对齐，写了一个Tbc，添加IMU积分后的数据还算稳定，IMU的加入使ORB-SLAM3能够完成对Roll、Pitch的估计，并对ORB特征点跟踪失败后仍能利用IMU的输出维持一段时间的定位。

5、ORB-SLAM3算法总结

目前能够编译运行诸如此类的特征法SLAM开源项目了，虽然初学经历了各种尝试，但是熟悉了Linux系统和ROS系统的操作、cmake、shell、OpenCV等等，为了能够自己进行开发，还需要更深入的学习。