基于深度摄像头的vslam（一）

（一）基础知识

slam基本概念

slam，即时定位与地图构建（SimultaneousLocalization AndMapping）指的是机器人在自身位置不确定的条件下,在完全未知环境中创建地图,同时利用地图进行自主定位和导航。

SLAM问题可以描述为:机器人在未知环境中从一个未知位置开始移动,在移动过程中根据位置估计和传感器数据进行自身定位,同时建造增量式地图。

（1）定位(localization)：机器人必须知道自己在环境中位置。
（2）建图(mapping)：机器人必须记录环境中特征的位置（如果知道自己的位置）。
（3）SLAM：机器人在定位的同时建立环境地图。其基本原理是运过概率统计的方法，通过多特征匹配来达到定位和减少定位误差的。

移动机器人自定位与环境建模问题是紧密相关的。环境模型的准确性依赖于定位精度，而定位的实现又离不开环境模型。已有的研究中对这类问题的解决方法可分为两类：一类利用自身携带的多种内部传感器(包括里程仪、罗盘、加速度计等)，通过多种传感信息的融合减少定位的误差，使用的融合算法多为基于卡尔曼滤波的方法。这类方法由于没有参考外部信息，在长时间的漫游后误差的积累会比较大。另一类方法在依靠内部传感器估计自身运动的同时，使用外部传感器(如激光测距仪、视觉等)感知环境，对获得的信息进行分析提取环境特征并保存，在下一步通过对环境特征的比较对自身位置进行校正。但这种方法依赖于能够取得环境特征。

基于视觉的Visual SLAM（简称VSLAM）框架

VSLAM的技术框架主要包括传感器数据预处理、前端、后端、回环检测、建图。

1、传感器数据
传感器数据预处理。这里的传感器包括摄像头、惯性测量单元（Inertial measurement unit，简称IMU）等，涉及传感器选型、标定、多传感器数据同步等技术。
深度摄像头：RGB-D，主动探测深度，重建效果好，测量范围小，受日光干扰，受材质干扰

2、前端
又称为视觉里程计（visual odometry，简称VO）。主要是研究如何根据相邻帧图像定量估算帧间相机的运动。通过把相邻帧的运动轨迹串起来，就构成了相机载体（如机器人）的运动轨迹，解决了定位的问题。然后根据估算的每个时刻相机的位置，计算出各像素的空间点的位置，就得到了地图。

VSLAM中，前端主要涉及计算机视觉相关的算法。典型做法一般是：首先提取每帧图像特征点，对相邻帧进行特征点粗匹配，然后利用RANSAC（随机抽样一致）算法去除不合理的匹配对，然后得到位置和姿态信息。整个过程涉及到特征提取、特征匹配、对极几何、PnP、刚体运动、李代数等多视图几何知识。

视觉里程计只计算相邻帧的运动，进行局部估计，这会不可避免的出现累积漂移，这是因为每次估计两个图像间的运动时都有一定的误差，经过相邻帧多次传递，前面的误差会逐渐累积，轨迹漂移（drift）的越来越厉害。

解决轨迹漂移的方法有两个：后端优化、回环检测。

3、后端
主要是对前端的结果进行优化，得到最优的位姿估计。主要有两种方法：

一种是基于滤波理论的优化，主要有 EKF, PF, RBPF, UKF等方法，其中EKF（扩展卡尔曼滤波）在早期是主流的方法。它的思路是将状态估计模型线性化，并用高斯分布近似其噪声，然后按照卡尔曼滤波进行预测来更新。但是实际上，这种对噪声的高斯分布大部分情况下是不成立的，此外，线性化过程中丢失了高阶项。

另一种就是非线性优化（图优化）。它的基本思想是将优化的变量作为图的节点，误差项作为图的边，在给定初值后，就可以迭代优化更新。由于图优化的稀疏性，可以在保证精度的同时，降低计算量。

后端优化涉及到的数学知识比较多，具有较高的难度。总的来说，从状态估计的角度来讲，SLAM是一个非线性非高斯系统。因此传统的滤波理论已经逐渐被抛弃，而图优化已经成为主流方法。

4、回环检测
主要目的是让机器人能够认识自己曾经去过的地方，从而解决位置随时间漂移的问题。视觉回环检测一般通过判断图像之间的相似性完成，这和我们人类用眼睛来判断两个相同的地点是一样的道理。因为图像信息丰富，因此VSLAM在回环检测中具有很大的优势。

当回环检测成功后，就会建立现在的图像和过去曾经见过图像的对应关系，后端优化算法可以根据这些信息来重新调整轨迹和地图，从而最大限度地消除累积误差。

5、建立地图
SLAM根据不同的传感器类型和应用需求建立不同的地图。常见的有2D栅格地图、2D拓扑地图、3D点云地图等。

2D拓扑地图更强调地图元素之间的连通关系，而对精确的位置要求不高，去掉了大量地图的细节，是一种非常紧凑的地图表达方式。3D点云地图在VSLAM中用的比较多，主要用于真实场景的视觉重建，重建的地图非常直观漂亮。但是点云地图通常规模很大，比如一张VGA分辨率（640 x 480）的点云图像，就会产生30万个空间点，这会占据非常大的存储空间，而且存在很多冗余信息。

通俗的思考一些问题

1、关于我们要做的事情，可以这样想：

（1） 已知的东西：传感器数据（图像，点云，惯性测量设备等）。我们的传感器主要是一个Kinect，因此数据就是一个视频序列，说的再详细点就是一个RGB位图序列与一个深度图序列。至于惯性测量设备，可以有也可以没有。

（2） 待求的东西：机器人的运动轨迹，地图的描述。运动轨迹，画出来应该就像是一条路径。而地图的描述，通常是点云的描述。但是点云描述是否可用于导航、规划等后续问题，还有待研究。

2、全局优化

如果只用帧间匹配，那么每一帧的误差将对后面所有的运动轨迹都要产生影响。例如第二帧往右偏了0.1，那么后面第三、四、五帧都要往右偏0.1，还要加上它们自己的估算误差。所以结果就是：当程序跑上十几秒之后早就不知道飞到哪儿去了。这是经典的SLAM现象，在EKF实现中，也会发现，当机器人不断运动时，不确定性会不断增长。

怎么办？

通常的做法是认准一个标志性建筑物，在它周围转上几圈，弄清楚附近的环境。然后再一点点儿扩大我们走过的范围。在这个过程中，我们会时常回到之前已经见过的场景，因此对它周围的景象就会很熟悉。

机器人的情形也差不多，除了大多数时候是人在遥控它行走。因而我们希望，机器人不要仅和它上一个帧进行比较，而是和更多先前的帧比较，找出其中的相似之处。这就是所谓的回环检测（Loop closure detection）。

回环检测，又称闭环检测（Loop closure detection），是指机器人识别曾到达场景的能力。如果检测成功，可以显著地减小累积误差。

没有回环时，由于误差对后续帧产生影响，机器人路径估计很不稳定。加上一些局部回环，几个相邻帧就多了一些约束，因而误差就减少了。你可以把它看成一个由弹簧连起来的链条（质点-弹簧模型）。当机器人经过若干时间，回到最初地方时，检测出了大回环时，整个环内的结构都会变得稳定很多。我们就可以籍此知道一个房间是方的还是圆的，面前这堵墙对应着以前哪一堵墙，等等。

其他

讲一下其他的乱七八糟的东西，毕设换题了……很悲哀了，查了一通很迷茫不知道怎么开始烦的一比，和我妈说我说我要回家，我妈第一反应好呀好呀，后来听说我毕设换题不相干之后她让我别回来了……憋说了是亲妈。

看资料这一群数学问题各个过程整个系统还是很懵，先整理了一些基础入门问题概念等等，明天跟着高博（网上发现的，真是个神仙学长）的教程跑一跑程序吧。想哭了又，这都什么神仙玩意……

参考博客

1. https://www.cnblogs.com/slamtec/p/9706557.html
2. https://blog.csdn.net/tiandijun/article/details/55212968