MATLAB算法实战应用案例精讲-【图像处理】SLAM技术详解

前言

几个高频面试题目

SLAM和路径规划对比

算法原理

SLAM组成

常用的SLAM传感器

常用的两种地图类型

SLAM算法实现4要素

主流SLAM算法

2D激光SLAM算法

1. Gmapping

2. Hector slam

3. KartoSLAM

4. LagoSLAM

5. CoreSLAM

3D激光SLAM算法对比

测试的SLAM方案

实验

1. museum_out

2. museum_in

3. outdoor3

4. outdoor4

5. aggressive

总结

存在的问题及困难

应用案例

SLAM基础环境配置

1.网络连接及软件源

2.基础开发环境确认与搭建

3.测试

3D激光SLAM-点云特征提取

本项目所用的特征提取方法说明

特征提取理论与代码编写

确定激光点所在的扫描线并建立点的索引

计算每个点的曲率

根据曲率来筛选特征点

分别将平面点和角点放在两个点云数据结构里

测试

应用场景

前言

SLAM(Simultaneous Localization and Mapping)同步定位与地图构建,一种同时实现设备自身定位和环境地图构建的技术。

原理是使用相机、激光雷达、惯性测量单元等传感器，来收集环境信息，然后用算法将这些信息融合起来，以确定设备在未知环境中的位置，并构建一张环境地图。

通俗解释为：

用眼睛观察周围地标如建筑、大树、花坛等，并记住他们的特征（特征提取）
在自己的脑海中，根据双目获得的信息，把特征地标在三维地图中重建出来（三维重建）
当自己在行走时，不断获取新的特征地标，并且校正自己头脑中的地图模型（bundle adjustment or EKF）
根据自己前一段时间行走获得的特征地标，确定自己的位置（trajectory）
当无意中走了很长一段路的时候，和脑海中的以往地标进行匹配，看一看是否走回了原路（loop-closure detection）。

视觉SLAM算法相对于激光SLAM算法的特点是信息更加丰富，由于是在二维提取特征点，因此通常可以达到更高的频率，但也正是因为信息丰富，因此更容易引入噪声，加上缺乏三维信息，导致视觉SLAM算法的鲁棒性在平均水平上要低于激光SLAM，尤其是通过传统特征进行定位和建图，在工程应用上相对受限，当前一个热门的方向是通过网络提取更加鲁棒的特征，例如SuperPixel、SuperGlue，或者直接根据网络输出定位和建图结果.

几个高频面试题目

SLAM和路径规划对比

slam+路径规划是一个完整的为了解决机器人在哪，去哪，怎么去的问题。

slam解决在哪和去哪，路径规划解决怎么去。

所以slam为了解决在哪和去哪需要建立地图和实现自我定位，路径规划则是以这个地图为基础实现全局规划(最优路径)和局部规划(避障)。

1、首先slam要构建地图涉及的算法就很多，而且比较难，比如:

机器人边扫边绘图，彼此之间的地图如何拼接？

绘图的时候如何和机器人运动状态进行关联，机器人离一堵墙5米进行测量和离10米测一堵墙，到底是一堵墙还是两堵墙？

庞大的激光雷达点云数据如何解算，存储和显示？

进行机器人定位的时候，特征点如何匹配？地图用什么样的地图，全局地图，矢量地图？

关于环境扫描是用激光雷达还是视觉相机？(貌似现在视觉slam比较热，论文期刊都比较青睐视觉slam)……等等

当然关于地图也可以离线自己绘制地图，不过实用性不强，一旦环境发生变化又要重新绘制，费时费力，不如开着机器人扫一圈来的方便，也可以对扫描后的地图进行涂鸦修改。

2、而路径规划则是基于已经创建好的地图来进行规划，就好比你已经有了地图，怎么从地图上找到一个合适的路径到达。基本包括

1）加载slam出来的地图

2）生成全局路径和局部路径

3）基于运动学和动力学进行实时规划

路径的选取有好多种算法，可以说是一种搜索算法。

路径规划主要是啃算法：比如全局路径算法:

A*算法，Dijkstra算法，Bellman-Ford算法，Floyd-Warshall算法，Johnson算法，SPFA算法……输出的是一条全局路线。这个不关心运动学和动力学。

局部路径规划一方面要有跟踪全局路线的能力，比如MPC预测模型，此时需要结合运动状态方程。另一方面要有一些避障算法，防止机器人在全局路线上突然遭遇障碍物。这个阶段关心运动学和动力学。

可以看出slam和路径规划其实是相辅相成的，slam也是为了后面的路径规划做准备。

slam难，就业面也相对窄一些，感觉没有路径规划的广，热度也没以前火爆了，可能是遇到了一些瓶颈，无人驾驶领域应该是slam的天花板了。

另一方面关于slam+路径规划的运用，像ROS会有对应的开发包，而且ROS平台的订阅消息的模式实现了激光雷达，IMU等传感器的数据流传输，而且数据流通过可以Rivz进行了展示。

不过ROS平台正儿八经的工程运用还不成熟，偏教育，实时性比较差，主节点挂掉会影响所有子节点运行，不过为学习应用slam确实省了不少事。这就要求开发者还要避开Ros这种平台，自己搭建平台调用slam开发包，对能力提出了更高的要求。

算法原理

不同的SLAM算法，实现的具体细节会有所不同，但一般都包含前端和后端。

前端：

从传感器中获取原始数据，并将这些数据与已有地图进行关联，从而确定机器人轨迹的过程。

数据采集：通过传感器获取机器人周围环境的数据，如激光点云数据、图像数据等。
数据时空同步：将从不同传感器或不同时间戳接收到的数据进行同步，以便后续配准。
特征提取：从采集的数据中提取用于建图的特征点，如关键点、特征描述等。
数据融合：将不同传感器获取的数据融合起来，提高建图的准确性和稳定性。
数据关联：将当前帧的特征与之前的地图，或者其他帧之间的特征进行匹配，以确定机器人的运动轨迹。
运动估计：通过数据关联得到机器人的运动轨迹，可以是平移、旋转等运动。

后端：

根据前端获取的运动轨迹和地图信息，对机器人的状态、地图和传感器误差等进行估计和优化的过程。

非线性优化：通过非线性最小二乘法等，对机器人姿态和地图进行优化，使得机器人的位置和地图更加准确。
回环检测：识别机器人经过的相似位置，避免累积误差的产生。可以有效降低机器人的定位误差，提高SLAM算法的精度和鲁棒性。

根据传感器的不同，SLAM算法可以分为二维激光SLAM、三维激光SLAM，以及视觉SLAM。

二维激光SLAM常用的有Cartographer、Karto，三维激光SLAM较流行的是LIO-SAM和LOAM系列，视觉SLAM主流的方案为ORB-SLAM3、VINS-Fusion

激光SLAM

激光SLAM发展已相对较为成熟，它的研究内容主要包括：传感器数据处理、前端配准方法、回环检测、后端优化、图优化、已知定位的建图、3D激光与视觉融合、多传感器融合。

光探测与测距（激光雷达）方法主要使用激光传感器（或距离传感器）。对比相机、ToF 和其他传感器，激光可以使精确度大大提高，常用于自动驾驶汽车和无人机等高速移动运载设备的相关应用。激光传感器的输出值一般是二维 (x, y) 或三维 (x, y, z) 点云数据。激光传感器点云提供了高精确度距离测度数据，特别适用于 SLAM 建图。一般来说，首先通过点云匹配来连续估计移动。然后，使用计算得出的移动数据（移动距离）进行车辆定位。对于激光点云匹配，会使用迭代最近点 (ICP) 和正态分布变换 (NDT) 等配准算法。二维或三维点云地图可以用栅格地图或体素地图表示。但就密度而言，点云不及图像精细，因此并不总能提供充足的特征来进行匹配。例如，在障碍物较少的地方，将难以进行点云匹配，因此可能导致跟丢车辆。此外，点云匹配通常需要高处理能力，因此必须优化流程来提高速度。鉴于存在这些挑战，自动驾驶汽车定位可能需要融合轮式测距、全球导航卫星系统 (GNSS) 和 IMU 数据等其他测量结果。仓储机器人等应用场景通常采用二维激光雷达 SLAM，而三维激光雷达点云 SLAM 则可用于无人机和自动驾驶。

视觉SLAM

使用从相机和其他图像传感器采集的图像。视觉 SLAM 可以使用普通相机（广角、鱼眼和球形相机）、复眼相机（立体相机和多相机）和 RGB-D 相机（深度相机和 ToF 相机）。视觉 SLAM 所需的相机价格相对低廉，因此实现成本较低。此外，相机可以提供大量信息，因此还可以用来检测路标（即之前测量过的位置）。路标检测还可以与基于图的优化结合使用，这有助于灵活实现 SLAM。使用单个相机作为唯一传感器的 vSLAM 称为单目 SLAM，此时难以定义深度。这个问题可以通过以下方式解决：检测待定位图像中的 AR 标记、棋盘格或其他已知目标，或者将相机信息与其他传感器信息融合，例如测量速度和方向等物理量的惯性测量单元 (IMU) 信息。vSLAM 相关的技术包括运动重建 (SfM)、视觉测距和捆绑调整。视觉 SLAM 算法可以大致分为两类。稀疏方法：匹配图像的特征点并使用 PTAM 和 ORB-SLAM 等算法。稠密方法：使用图像的总体亮度以及 DTAM、LSD-SLAM、DSO 和 SVO 等算法。

视觉SLAM整体流程大致可以概括为五步：

（1）传感器信息读取

在视觉SLAM中主要为相机图像信息的读取和预处理。如果是在机器人中，还可能有码盘、惯性传感器等信息的读取和同步。

（2）视觉里程计 (Visual Odometry,VO)

视觉里程计的任务是估算相邻图像间相机的运动，以及局部地图的样子。VO又称为前端(FrontEnd)。

（3）后端优化(Optimization）

后端接受不同时刻视觉里程计测量的相机位姿，以及回环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于接在VO之后，又称为后端(Back End)。

（4）回环检测(Loop Closing)

回环检测判断机器人是否到达过先前的位置。如果检测到回环，它会把信息提供给后端进行处理。

（5）建图(Mapping)

它根据估计的轨迹，建立与任务要求对应的地图。

其整体流程图大致如下

SLAM组成

传感器：激光雷达、摄像头、惯性测量单元（IMU）、里程计等。这些传感器可以提供设备在运动过程中所需要的信息，如距离、角度、速度、方向等。
设备运动模型：运动模型用于估计设备的位姿，例如里程计模型、速度模型、加速度模型等。
视觉算法：视觉算法主要用于设备在环境中的感知和定位，如特征提取、特征匹配、图像配准等。
滤波算法：滤波算法用于估计设备的位姿和地图中不同位置的概率分布，如卡尔曼滤波器、粒子滤波器等。
优化算法：优化算法用于优化设备的位姿和地图，如扩展信息滤波器、位姿图优化等。
地图表示：地图表示方式可以有多种，如栅格地图、拓扑地图、点云地图等。

常用的SLAM传感器

激光雷达：激光雷达是一种通过发射激光束来探测目标并测量距离的传感器。它可以提供高精度、高分辨率的距离和角度信息，被广泛应用于SLAM中。
摄像头：摄像头可以通过拍摄环境中的图像来获取场景的信息，如特征点、轮廓等。在SLAM中，摄像头可以用于实现视觉SLAM或者辅助其他传感器进行定位。摄像头种类有单目、双目、单目结构光、双目结构光、ToF。
惯性测量单元（IMU）：IMU可以测量机器人的加速度和角速度等信息，通过积分可以得到机器人的位姿信息。IMU通常与其他传感器结合使用，用于实现多传感器融合。
里程计：里程计是一种通过测量轮子旋转的信息来推算机器人位移的传感器。虽然里程计的精度较低，但在SLAM中仍然是一个非常重要的传感器。

此外，还有其他一些传感器，如GPS、磁力计等，可以用于SLAM中。不同的传感器有不同的特点和适用场景，根据具体的应用需求选择合适的传感器非常重要。

常用的两种地图类型

覆盖栅格地图（Occupany Grid Map）

一种常见的地图表示方式，主要用于描述环境中每个位置的占据状态。在覆盖栅格地图中，环境被分成一个个网格单元，每个网格单元表示一个位置，并记录该位置的占据状态，一般用二进制值表示，例如0表示空闲，1表示占据。

覆盖栅格地图可以通过激光雷达等传感器获得环境中障碍物的位置信息，将障碍物所在的网格单元标记为占据状态。同时，通过机器人的位姿信息，可以将机器人所在的网格单元标记为占据状态，并在地图中绘制出机器人的轨迹。

覆盖栅格地图的优点在于简单、易于实现和可视化，可以用于快速构建地图和进行导航。但是，由于覆盖栅格地图要求环境被划分为网格单元，因此地图的精度和分辨率可能会受到限制。同时，覆盖栅格地图还需要消耗大量的存储空间和计算资源，因为需要维护大量的网格单元。

需要注意的是，覆盖栅格地图只记录了占据状态，并没有记录障碍物的形状和大小等信息。如果需要更精确的地图表示方式，可以使用其他地图表示方法，如点云地图、边缘地图等。

点云地图（Point Cloud Map）

主要用于描述环境中物体的三维形状和位置信息。在点云地图中，环境被表示为一组三维点云，每个点表示一个位置，并记录该位置的属性信息，如颜色、方向量、反射率等。

点云地图可以通过激光雷达、摄像头等传感器获得环境中物体的位置和形状信息，通过点云配准和滤波等算法，可以将多个点云融合成一个完整的地图，用于机器人的定位、导航和避障等任务。

相对于覆盖栅格地图等二维地图表示方式，点云地图的优点在于可以提供更加真实的环境信息，可以描述物体的三维形状和位置信息，同时也可以用于识别和分类物体。但是，点云地图的缺点在于数据量较大，需要消耗大量的存储空间和计算资源，同时对点云数据的处理和配准等算法也比较复杂。

需要注意的是，点云地图的精度和分辨率取决于传感器的精度和采样率等因素，而点云地图的质量对机器人的定位和导航等任务影响很大，因此需要对点云地图进行精细的处理和优化。

SLAM算法实现4要素

地图表示，比如dense和sparse都是它的不同表达方式，这个需要根据实际场景需求去抉择
信息感知，需要考虑如何全面的感知这个环境，RGBD摄像头FOV通常比较小，但激光雷达比较大
数据关联，不同的sensor的数据类型、时间戳、坐标系表达方式各有不同，需要统一处理
定位与构图，就是指怎么实现位姿估计和建模，这里面涉及到很多数学问题，物理模型建立，状态估计和优化

其他的还有回环检测问题，探索问题（exploration），以及绑架问题（kidnapping）。

主流SLAM算法

Cartographer

由谷歌开发的一款基于激光雷达和RGB-D相机数据的SLAM算法。可以跨平台使用，支持Lidar、IMU、Odemetry、GPS、Landmark等多种传感器配置，被广泛用于机器人导航、自动驾驶等领域。

Cartographer系统架构图

Cartographer算法在前端完成占据栅格地图的构建，得出激光雷达扫描帧的最佳位姿后，将扫描帧插入到子地图Submap中，得到局部优化的子地图并记录位姿。

后端根据扫描帧间的位姿关系进行全局的地图优化，并使用分支定界法加速求解，进而得出闭环扫描帧在全局地图中的最佳位姿。

Karto

一种基于位姿图优化的SLAM方法，使用了高度优化和非迭代的cholesky矩阵对系统进行解耦并求解。适用于各种室内环境，可以处理静态和动态障碍物。

Karto系统架构图

Karto使用图论的标准形式表示地图，其中每个节点代表了移动机器人运行轨迹上的一个位姿点，以及当前位姿下传感器返回的感知信息。

节点之间的边，代表了相邻机器人位姿之间的位移矢量。对每一个新的位姿点定位，需要节点间匹配关系和边带来的约束，保持定位估计误差的前后一致。

LIO-SAM

一种新型的激光惯性导航系统，结合了激光雷达和惯性测量单元数据，可以实现机器人的高精度定位和运动轨迹的建图。

LIO-SAM系统架构图

前端在传统的LIDAR-SLAM基础上，利用卡尔曼滤波和因子图优化算法，将激光雷达和IMU数据融合，进一步提高机器人的定位精度和建图效果。后端加入优化算法，使得机器人的定位精度和建图精度都得到极大提升。

LOAM系列

一种成熟的基于激光雷达的SLAM算法，包括LOAM、LOAM-Velodyne、LOAM-LiDAR等。

其中LOAM使用3D激光雷达的数据来进行建图和定位，利用点云数据的特征，如空间聚类、连续性约束等来估计位姿。

LOAM系统架构图

LOAM-Velodyne则使用Velodyne激光雷达的3D点云数据，以实现更高精度的地图重建和定位；LOAM-LiDAR采用LiDAR传感器来进行建图和定位，可以高效获取目标物体的三维坐标信息，在一些机器人应用场景具有很大的优势。

基于LOAM系统的地图构建局部

ORB-SLAM3

当前最优秀的基于特征点的视觉SLAM系统之一，支持单目、双目、RGB-D等多种相机模式，在特征提取、关键帧选取、地图维护、位姿优化等方面进行了优化，并能建立短期、中期和长期的数据关联，使得该系统兼具精度和鲁棒性。

ORB-SLAM3系统架构图

前端视觉里程计基于ORB特征，建立图像帧之间特征点的数据关联，以及图像特征点和地图点之间3D到2D的数据关联，具有较好的鲁棒性和提取效率，回环检测和重定位也基于ORB特征实现。

VINS-Fusion

一种基于视觉惯性传感器的视觉SLAM算法。它将视觉和惯性信息进行融合，来提高机器人在未知环境下的定位和导航能力。

VINS-Fusion系统架构图

前端和后端之间通过一个状态传递机制来交换数据。前端将IMU和图像数据合并，生成一组状态量，然后将其传递给后端进行融合和优化；后端将优化后的状态量传递回前端，以便更新机器人的运动估计。

SLAM方案选择

1. 二维SLAM就适合在二维平面上运动的机器人，如扫地机、配送机器人、迎宾机器人等。

具体来看，Cartographer的总体表现最为优秀，定位精度和建图质量都很高，同时鲁棒性也很强，能承受一定的噪声和漂移干扰，可以应用于大范围建图；Karto适合建图面积不是太大的低成本应用场景，可以在一定程度上满足建图需求，同时工程实现难度也不太高。

2. 三维SLAM则适用于机器人需要在三维空间中进行建模和定位的场景，比如无人机在室外飞行、机器人在户外环境中进行探测等。

其中，LIO-SAM的鲁棒性和实时性表现相对优秀，适用于建图面积和硬件资源有限的小型机器人应用场景；在建图面积和鲁棒性方面比较弱的LOAM方案，适用于小而快速的机器人及小型场景的定位、建图任务。

3.视觉SLAM通过摄像头等可视传感器来获取机器人与环境之间的相对位置关系，不需要其他传感器，更为轻量化。但对于光照变化、运动模糊等情况的处理仍存在挑战。

定位精度和实时性都比较高的ORB-SLAM3，面对场景变化能够快速适应，适用于多种场景的建图。与之相比，VINS-Fusion适用于对定位精度要求较高，且建图面积相对较小的机器人应用场景。

总的来说，这些算法各有优劣，如何选择最合适的算法，需要根据具体的应用场景和实际需求来综合考虑。

2D激光SLAM算法

1. Gmapping

Gmapping是应用最为广泛的2D slam方法，主要是利用RBPF（Rao-Blackwellized Particle Filters）方法，所以需要了解粒子滤波的方法（利用统计特性描述物理表达式下的结果）。Gmapping在RBPF算法上做了两个主要的改进：改进提议分布和选择性重采样。

Gmapping可以实时构建室内地图，在构建小场景地图所需的计算量较小且精度较高。相比Hector SLAM对激光雷达频率要求低、鲁棒性高（Hector 在机器人快速转向时很容易发生错误匹配，建出的地图发生错位，原因主要是优化算法容易陷入局部最小值）；而相比Cartographer在构建小场景地图时，Gmapping不需要太多的粒子并且没有回环检测因此计算量小于Cartographer而精度并没有差太多。

随着场景增大所需的粒子增加，因为每个粒子都携带一幅地图，因此在构建大地图时所需内存和计算量都会增加。因此不适合构建大场景地图。并且没有回环检测，因此在回环闭合时可能会造成地图错位，虽然增加粒子数目可以使地图闭合但是以增加计算量和内存为代价。

所以Gmapping不能像cartographer那样构建大的地图，虽然论文生成几万平米的地图，但实际我们使用中建的地图没有上万平米时就会发生错误。

优点：在长廊及低特征场景中建图效果好；

缺点：依赖里程计（odometry），无法适用无人机及地面小车不平坦区域；无回环；

2. Hector slam

Hector slam对传感器的要求比较高，它主要是利用高斯牛顿方法来解决scan-matching的问题。

Hector slam无需使用里程计，所以在不平坦区域实现建图的空中无人机及地面小车具有运用的可行性，利用已经获得的地图对激光束点阵进行优化，估计激光点在地图的表示，和占据网络的概率。获得激光点集映射到已有地图的刚体变换，为避免局部最小而非全局最优出现，地图使用多分辨率。

需具备高更新频率且测量噪音小的激光扫描仪，所以，在制图过程中，robot的速度要控制在较低的情况下才会有比较理想的建图效果，这也是它没有回环的一个后遗症。另外在里程计数据比较精确的情况下无法有效利用里程计信息。

优点：不需要使用里程计，所以使得空中无人机及地面小车在不平坦区域建图存在运用的可行性；利用已经获得的地图对激光束点阵进行优化, 估计激光点在地图的表示,和占据网格的概率；利用高斯牛顿方法解决scan-matching 问题，获得激光点集映射到已有地图的刚体变换；为避免局部最小而非全局最优，使用多分辨率地图；导航中的状态估计加入惯性测量系统（IMU），利用EKF滤波；

缺点：需要雷达（LRS）的更新频率较高，测量噪声小。所以在制图过程中，需要robot速度控制在比较低的情况下，建图效果才会比较理想，这也是它没有回环（loop close）的一个后遗症；且在里程计数据比较精确的时候，无法有效利用里程计信息。

实验表明：在大地图，低特征（distinctive landmarks）场景中，hector的建图误差高于gmapping。这是由于hector过分依赖scan-match。特别是在长廊问题中，误差更加明显。

补充：hector_slam通过最小二乘法匹配扫描点，且依赖高精度的激光雷达数据，因此扫描角很小且噪声较大的Kinect是不行的，匹配时会陷入局部点，地图比较混乱。

3. KartoSLAM

KartoSLAM是基于图优化的方法，用高度优化和非迭代 cholesky矩阵进行稀疏系统解耦作为解，图优化方法利用图的均值表示地图，每个节点表示机器人轨迹的一个位置点和传感器测量数据集，箭头的指向的连接表示连续机器人位置点的运动，每个新节点加入，地图就会依据空间中的节点箭头的约束进行计算更新．

KartoSLAM的ROS版本，其中采用的稀疏点调整（the Spare Pose Adjustment(SPA)）与扫描匹配和闭环检测相关。landmark越多,内存需求越大，然而图优化方式相比其他方法在大环境下制图优势更大，在某些情况下KartoSLAM更有效,因为他仅包含点的图(robot pose)，求得位置后再求map。

4. LagoSLAM

LagoSLAM是线性近似图优化，不需要初始假设，优化器的方法可以有三种选择 Tree-based netORK Optimizer(TORO), g2o,LAGO。

基本的图优化slam的方法就是利用最小化非线性非凸代价函数，每次迭代，解决局部凸近似的初始问题来更新图配置，过程迭代一定次数直到局部最小代价函数达到， (假设起始点经过多次迭代使得局部代价函数最小)。

5. CoreSLAM

为了简单和容易理解最小化性能损失的一种slam算法，将算法简化为距离计算与地图更新的两个过程，第一步，每次扫描输入，基于简单的粒子滤波算法计算距离，粒子滤波的匹配器用于激光与地图的匹配，每个滤波器粒子代表机器人可能的位置和相应的概率权重，这些都依赖于之前的迭代计算. 选择好最好的假设分布，即低权重粒子消失，新粒子生成，在更新步骤，扫描得到的线加入地图中，当障碍出现时，围绕障碍点绘制调整点集，而非仅一个孤立点。

通过在大小仿真环境、实际环境以及CPU消耗的情况下进行算法比较，发现KartoSLAM 与gmapping优势更大。

3D激光SLAM算法对比

测试的SLAM方案

共测试了github上开源的8种方案，按照特点可分为

特点	方案
纯Lidar	A-LOAM(港科大版本的LOAM)，hdl_graph_slam，BLAM
Lidar与IMU松耦合	LeGo-LOAM，SC-LeGo-LOAM(在LeGo-LOAM上使用了一种新的回环检测方法)
Lidar与IMU紧耦合	LINS，LIO-SAM，LIOM

说明

上述方案中除了hdl_graph_slam和BLAM外，其余方案都是基于LOAM或LeGo-LOAM
在实验中，hdl_graph_slam和BLAM在所有数据集上的性能均不理想，因而下面不再讨论。而SC-LeGo-LOAM的性能较之LeGo-LOAM也没有明显改善，因而下面也不再讨论。

原生Demo

在第二部分的实验结果展示中，为了方便比较，不同方案得到的结果形式都进行了统一化，但实际上各种方案在执行时的视觉效果是不同的，这里展示了利用各种方案的原生配置所得到的demo，日后可根据需要配置成下面的任意一种效果：

实验

实验共使用了5段在伟清楼附近采集的数据：

数据段名称	采集时间	传感器配置	路程	备注
museum_out	2020.7.25	10Hz Lidar+ 100Hz IMU	639m	无
museum_in	2020.7.25	10Hz Lidar+ 100Hz IMU	226m	回环
outdoor3	2020.7.27	10Hz Lidar+ 400Hz IMU	481m	回环
outdoor4	2020.7.27	10Hz Lidar+ 400Hz IMU	562m	回环
aggresive	2020.7.27	10Hz Lidar+ 400Hz IMU	138m	回环，剧烈运动

说明

两日的数据：由于在7.25采集的两段数据中没有明显看出加入IMU的优势，于是在7.27我将IMU频率调高至LINS，LIOM和LIO-SAM所采用的400Hz后又采集了三段数据。
IMU的校正：在每次采集数据前，我都将IMU的z轴与重力加速度重新对齐，方法是利用手机软件（如：水平仪，AIDA64等）测量，寻找到一个较好的水平面，将实验设备置于其上，利用Mtmanager对IMU进行Alignment Reset，实现z轴的重置。
IMU初值：IMU紧耦合算法中需要提供IMU的初始零偏（bias）和噪声方差，而每段数据在开始时的一段时间内实验设备都是静止的，因而我利用这段时间内（约5s）的IMU测量值进行了零偏和噪声的估计。需要注意的是，由于IMU是粘贴在Lidar外壳上的，而Lidar在扫描时会引起外壳周期性的振动，这也反映在了IMU的测量值中，如下图：

x轴和y轴的加速度ax，ay和角速度wx,wy有明显的周期性，周期为雷达的扫描周期。而经过进一步的DFT分析，可以发现ax，ay，wx，wy，wz在10Hz和40Hz的频谱分量最高，说明雷达振动的基波和4次谐波影响较大。为了去除振动影响，我暂时采取了时域拟合的方法，利用基波和4次谐波的组合来拟合测量信号，使得总误差最小，然后将拟合出的振动信号从测量信号中减去，将所得信号的均值和方差作为IMU的零偏和噪声方差。

1. museum_out

采集路线：从艺术博物馆停车场出发，环绕外围一周后来到伟清楼旁空地。

定位结果：

建图结果：

结论：
- 从定位结果上看，各种算法在x，y的估计上比较接近，分歧主要在高度z的估计上。根据实际情况，起点和终点的高度差并不是很大，LIO-SAM和LeGo-LOAM在这点上性能较好，其余方案则略有不足。
- 从建图结果上看，各算法的建图效果相当，只是LINS和LeGo-LOAM的地图稍微稀疏一些。

2. museum_in

采集路线：从艺术博物馆水池出发，在内场游走后回到原点：

定位结果：

建图结果：

结论：
- 从定位结果看，LIOM与其他方法有较大偏移，但轨迹的形状却是相似的，这是由于LIOM对IMU和Lidar外参进行了校正，使得坐标系有所偏移。ALOAM和LIOM很好地闭合了回环，但LIO-SAM，LINS和LeGo-LOAM的性能也还不错，也接近闭合。
- 从建图结果上看，各算法的建图效果相当，只是LINS和LeGo-LOAM的地图稍微稀疏一些。

3. outdoor3

采集路线：从伟清楼出发，环绕附近建筑一周后返回原点：

定位结果：

建图结果：

结论：
- 从定位结果来看，IMU紧耦合且存在回环检测的LIO-SAM成功地闭合了回环，其他方案都发生了明显漂移，其中，LeGo-LOAM在z方向产生了相当离谱的估计，这是由于点云中地面点较少，不能很好地约束住z方向,而使用了IMU紧耦合的另外两种LINS,LIOM较之ALOAM漂移较少，证明高频IMU起到了一定的约束作用。
- 从建图结果来看，LIO-SAM由于闭合了回环，建立了全局一致的地图。而其他方案建立的地图中则可以看出明显的漂移，例如图中左下角显示了起点处地图的侧视图，可以看到除LIO-SAM外，其他方案的地图都出现明显的分层现象，这就是z方向漂移的结果。

4. outdoor4

采集路线：从伟清楼出发，与outdoor3方向相反地环绕附近建筑一周后返回原点：

定位结果：

建图结果：

结论：
- 从定位结果来看，只有LIO-SAM很好地闭合了回环。LeGo-LOAM再次发生了退化，估计效果不佳，而LINS和LIOM在z方向产生了明显漂移，ALOAM在y和z方向都产生了明显漂移，说明IMU还是起到了一定的约束作用。
- 从建图结果来看，LIO-SAM得到了全局一致的地图，而其他方法由于漂移建图效果不太理想，例如图中的左下部分显示了起点处地图的俯视图，可以看到除LIO-SAM外都存在分层现象，这是z方向漂移的结果，而ALOAM和LeGo-LOAM还可以看出明显的平行相似结构（如柱子和右侧墙壁），这是y方向漂移的结果。

5. aggressive

采集路线：从伟清楼出发，在伟清楼，英士楼和刘卿楼围城的空地上行走，途中伴随有跑步后骤停，猛烈转弯等剧烈动作，最终回到原点：

定位结果：

建图结果：

结论：
- 从定位结果来看，LIO-SAM和LIOM很好地闭合了回环。LINS在z方向产生了明显漂移，ALOAM在y和z方向都产生了明显漂移，LeGo-LOAM则索性崩溃，这说明IMU还是起到了一定的约束作用。
- 从建图结果来看，采用了IMU紧耦合的三种方案LIO-SAM和LIOM和LINS得到的地图看起来都还不错，很难看出漂移或失真痕迹，而ALOAM和LeGo-LOAM的结果就相当糟糕了！

总结

各种方案的优缺点如下：

方案	优点	不足
ALOAM	1. 在几何特征丰富时比较稳定	1. 后期内存会出现爆炸，计算效率下降 2. 在几何特征较少时会产生明显漂移
LeGo-LOAM	1. 在地面点丰富时比较稳定 2. 轻量级	1. 在地面点缺乏时很容易崩溃 2. 得到的地图比较稀疏
LINS	1. 轻量级	1. z方向漂移明显 2. 得到的地图比较稀疏 3. 目前的版本要求Lidar与IMU体坐标系的xy平面平行，不接受自己提供的外參
LIO-SAM	1. 存在回环检测，能较好地闭合回环 2. 稳定性强 3. Demo看起来比较舒服	1. 在几何特征丰富的情况下可能不如ALOAM
LIOM	1. 存在重力加速度的校正和IMU初始状态估计	1. 稳定性不好，有时性能好，有时又不行，可能与其初始化环节的性能有关 2. 内存占用大，时间性能较差

在进行IMU校正后，融合高频IMU确实能够提升SLAM性能，尤其是在几何特征缺乏或者剧烈运动的情况下。
LIO-SAM在定位和建图方面做的都不错，比较建议使用。

存在的问题及困难

虽然 SLAM 已在某些场景下投入实际应用，但是仍面临诸多技术挑战

（1）定位误差累积，导致与实际值产生偏差

SLAM会估计连续移动，并容许一定的误差。但是误差会随着时间累积，导致与实际值产生明显偏差。误差还会导致地图数据瓦解或失真，让后续搜索变得困难。我们来看一个绕正方形通道行驶的例子。随着误差累积，机器人的起点和终点对不上了。这称为闭环问题。这类位姿估计误差不可避免。我们必须设法检测到闭环，并确定如何修正或抵消累积的误差。对策之一是记住之前到过的某处的某些特征，将其作为路标，从而最小化定位误差。构建位姿图有助于修正误差。将误差最小化问题视为优化问题进行求解，以生成更准确的地图数据。这种优化在视觉 SLAM 中称为捆绑调整。

（2）定位失败，地图上的位置丢失

图像和点云建图不考虑机器人的移动特征。在某些情况下，这种方法会生成不连续的位置估计。例如，可能会有计算结果显示，以 1 米/秒速度移动的机器人突然向前“瞬移”了 10 米。避免这种定位失败的办法有两种：一是使用恢复算法；二是将运动模型与多个传感器融合，以基于传感器数据计算。有多种方法可以实现运动模型的传感器融合。一种常见方法是使用卡尔曼滤波进行定位。由于大部分差速驱动机器人和四轮车辆一般都使用非线性运动模型，因此通常会使用扩展卡尔曼滤波器和粒子滤波器（蒙特卡罗定位）。某些情况下，也可以使用无迹卡尔曼滤波器等更加灵活的贝叶斯滤波器。一些常用传感器是惯性测量装置，例如惯性测量单元 (IMU)、航姿参考系统 (AHRS)、惯性导航系统 (INS)、加速度计传感器、陀螺仪传感器和磁力传感器。安装到车辆的轮式编码器通常用于测距。定位失败时，一种恢复对策是记住之前经过的某个位置的关键帧，将其作为路标。搜索路标时，会以特定方法进行特征提取以便高速扫描。有些方法基于图像特征，例如特征袋 (BoF) 和视觉词袋 (BoVW)。近年来，人们也使用深度学习来比较特征距离。

（3）图像处理、点云处理和优化带来高计算成本

在车辆硬件上实现 SLAM 时，计算成本是个问题。计算通常在处理能力有限的紧凑型低功耗嵌入式微处理器上执行。为了实现准确定位，必须高频率执行图像处理和点云匹配。此外，闭环等优化计算都是高成本计算流程。此处的挑战在于如何在嵌入式微处理器上执行这种高成本处理。对策之一是并行运行多个不同流程。例如，用于匹配流程前处理的特征提取就相对适合并行运行。使用多核 CPU 进行处理时，单指令多数据 (SIMD) 计算和嵌入式 GPU 在某些情况下可以进一步提升速度。而且，由于位姿图优化可以在相对长的周期里执行，降低其优先级并以规律间隔执行也能提高性能。

应用案例

SLAM基础环境配置

1.网络连接及软件源

网络连接

一个新系统的网络，可能需要配置一下网关才能够连接到现有网络上。如果直接能够连到网络上，可以跳过这一条。插上网线或者确认wifi后，在右上角网络配置里面选择需要的网络进行'Edit'，确认IPV4的信息正常（一般配置192.168.1.XXX等类似的模式），便可以进行正常网络访问，下次重启时也会自动连接。

软件源设置

可以通过左上角的search按钮搜'Software & Updates'，或者在'System Settings'里面找到这一项。主要修改两个地方，如下图。

一个是把从网络上下载源码勾选上，另一个是源选项，可以根据网络情况来自动选择源，也可以使用上交，清华，华中，阿里等的源，这里不会选的话建议自动。一般有点经验的话，直接修改镜像下载地址也可以。

更改好后，执行'sudo apt update'、'sudo apt upgrade -y'两条命令进行更新，大约几分钟就可以。

2.基础开发环境确认与搭建

配置好上述几点，你就可以按照自己的习惯来处理一下眼前这个单薄的系统了。举几个基本命令，可以查看一些系统信息，与windows任务管理的使用方法有些类似。如：使用'ifconfig'可以确认网络信息，使用'df'命令可以确认磁盘，使用'top'可以确认cpu及进程等。而SLAM技术的开发环境，根据功能需求或编程语言等划分也各有不同，此处举例说明一些基础库，IDE的确认与配置。

SLAM通用类别

①g++/gcc。作为最基础的编译工具，确认好它的版本是很关键的（命令行gcc/g++ -v 或者gcc/g++ -version）。从4.8到5.4再到更高级的版本，对于相同的源码编译出来的结果，或者说能否通过编译正确链接，是影响很大的，一般来说4.8.5和5.4.0是网上大部分代码的可用编译器版本，这里建议两个都安装，平常使用5.4.0，如果遇到低版本兼容问题（难点在于bug发生时能否顺利发现是因为编译器版本的问题，很可能某个模块高低版本都能编过，运行也都可以，但是有细小的不可捉摸的错误，很折磨），需要切换gcc版本编译，切换方法可以自行搜索。如果需要高版本适配，临时使用也是可以的；

②boost。boost是一个大类，含有丰富的功能，这里直接给出安装命令'sudo apt install libboost-all-dev -y'，它主要把C++各个函数和你的模块协调起来，增强你对API的调用，实现功能，随便举个例子：多线程。确保有这个库，让你事半功倍。

③Eigen。Eigen是矩阵库，SLAM中大量的矩阵运算，以及用于数学计算的向量，多维数组等，Eigen提供了非常好的模板。理解起来就像是把线性代数带入了C++，让一步步累加起来繁琐的步骤可以直接通过矩阵变换或者类似的原理简便起来，例如：向量的点积叉积，雅可比黑塞矩阵运算等。安装命令：'sudo apt install libeigen3-dev -y'。

④PCL。PCL库是点云库，PCL可以对点云进行API式的操作，包括各种滤波，特征提取，聚类，旋转平移变换，可视化等十分强大的功能，在激光SLAM算法中堪称使用最多的库，甚至可以不加之一俩字。为了避免使用出错，PCL库的安装尽可能的选择编译源码，因此它并不是像别的确认一下或者随手一个命令就能解决的了的，在Github的PCL仓能看到，目前有1.7，1.8，1.9，1.10，1.11几个不同版本，以安装差异来区别：其中1.7是属于一类，1.8，1.9，1.10属于另一类，1.11属于新一类。此处安装差异指以源码编译方式安装或移植造成可能的错误。

⑤OpenCV。OpenCV库是主要处理视觉图像的库，在视觉SLAM算法中也是使用最多的一类，对此也要确认2系列和3系列的区别，由于接口和API的相当一部分不同，在代码写作时也会有较大差异，安装时尽可能计划清楚。安装命令'sudo apt install libopencv-dev'（一般镜像源上的OpenCV是2系列）

⑥Ceres。SLAM算法中一个著名的区分方向就是后端优化，如何用手上粗糙的数据给出精准的预测，或者是把数据融合成一个能够让人信服的结果，就是后端优化经常考虑的问题。而Ceres库就是为了后端优化而生，它本身源于谷歌，安装的时候也是从GitHub上下载源代码进行编译使用，因为它的安装还要求了一些依赖项，使用起来也会有比较多的说法。计划后续也会开一个专题来讲这个Ceres库的应用。

另外还有诸如g2o，py系列的一些工具等也比较常用，由于使用经验不足，就不在这里多说，且行且学习，相关问题可以交流。

IDE

IDE种类很多，见过多屏多IDE的代码大佬，也见过用gedit的魔幻选手，还有使用vi的祖师爷，不过就使用上来讲，还是固定一两个IDE来比较好。部分IDE举例：VSCode，QT Creator，Kdevelop，CLion等等。这里就稍微说说VSCode和QT Creator，因为每一个IDE从安装到配置，再到使用时的额外附加项，其实都有挺多坑，如果有必要的话，后期再聊，这里就是一个普通的安装科普。

①VSCode。VSCode现在是本人最喜欢的开发环境，安装简单，随手一个官网，就有详细的安装步骤。这里提出来说是因为VSCode集成了许多功能插件，写代码方面的帮助非常强大，还有一些奇妙的黑科技，甚至一些摸鱼奇招，感兴趣可以自己找，有空的话再多介绍。需要注意的是几个配置的json文件，包括寻库路径，编译参数等等，如果一个大工程是基于VSCode开发的话，代码移植性也会由于json文件的存在而提高许多，另外VSCode的插件版本也可以一同管理着大家的公共部分版本，在以控制变量法解决问题时非常有帮助，各模块版本管理也就方便了起来。

②QT Creator。QT Creator个人感觉是一个更加专业的代码开发环境，有点类似于windows下的VS，它的强大之处在于图形化界面开发以及工具开发，具备非常优秀的辅助调试能力，其特有的qmake也可以让代码安全等级更高，在配置中具备更多的高级选项，启动时可以加载触发文件就能浏览整个工程，也很方便。如果说VSCode像是一匹战马，你可以自由地更改方向和速度，那么QTCreator就像是一部战车，威力强大，装甲优良。个人使用过程中唯一遇到的难点就是QTCreator在包含部分头文件/库的时候，跳转功能会卡住甚至未响应。

③Kdevelop。安装简单，方便上手，在需要打开项目工程代码的时候，没有良好配备的VSCode或者QT，也不想用vim，nano甚至是诡异的gedit，Kdevelop也是个不错的选择。使用命令'sudo apt-get install Kdevelop-dev'。

IDE的选择，根据实际需来决定，就像是VSCode和QT Creator在本身上的一些区别一样，需要什么就用什么，甚至无图形界面的vi也是一代开发板编辑传奇呢，实在不行gedit也是可以完成任务的。

3.测试

好啦，安装好了各种环境，不试一下怎么能行呢。随手写一个Hello World，检测一下你的编码能力吧。

std::cout << "Hello World!" << std::endl;

3D激光SLAM-点云特征提取

首先要弄明白为什么要进行特征提取。搞过2D激光SLAM的人都晓得，在进行扫描匹配的时候，经常是直接使用原始点云数据。这是因为一般2D激光SLAM所用的单线激光雷达每一帧点云数非常的少，一般几千个点都算多的了。因此点云中的所有点都可以参与运算，性能不会有大的问题。

而3D激光SLAM中所用的一般是多线激光雷达，每一个数据帧中的点云数量非常的多。像KITTI数据集中所用的Velodyne 16线激光雷达，一般点云数在几万。而Velodyne 64线激光雷达点云数量更是多达十几万。这个数量太过庞大，如果每一个点都参与运算，那么对计算资源的需求将是不能接受的。

点云特征提取就是为了尽可能降低参与运算的点的数量。在3D激光中，虽然特征提取不像视觉中那么方便，但是点云的空间结构化信息是可以利用起来的。最常见的有利用法向量提取特征(典型的PFH)，利用曲率提取特征(loam及其一系列衍生方法所用的方法)。

本项目所用的特征提取方法说明

在本项目中，将参考loam方法中使用的特征提取方法。loam长盛不衰，与其简洁又高效的的特征提取方式脱不开关系，因此极具借鉴意义。当然，本人也将探索新型的特征提取方法，特别是基于深度图(Range Image)的特征提取方法非常有吸引力，这个后续会另开分支，单独介绍这种方法。

特征提取理论与代码编写

特征提取这块原理比较简单，主要麻烦点在代码编写中，这个后面会叙述。loam及其衍生型的特征提取方法都是对原始点云提取Corner Point和Plane Point。而提取特征点的依据就是曲率。

那么重点就在于如何计算曲率了。

根据loam论文中所述，在计算某一点的曲率时，需要找到该点前后五个点（包括该点，一共11个点）一起来计算，计算公式为：

实际在编程的过程中要注意的地方在于如何确定集合 S 。这个后面会详细说。这里列出特征提取的主要步骤：

确定激光点所在的扫描线并建立点的索引

在loam系列方法中，会根据激光点的垂直方向的出射角确定出它在哪个扫描线上。第 i 个点所在的角度为：

此外我们还需要查阅手册确定出激光雷达在垂直方向的视场角以及垂直方向的角度分辨率来确定角度和线数的关系。以KITTI所用的Velodyne为例，其垂直视场角范围是30°(+15°~-15°)，角度分辨率为2°。

代码如下：

   int scan_idx = -1;
 typename CloudType::PointType::ValueType angle = ::atan2( pt.z, ::sqrt( pt.x * pt.x + pt.y * pt.y ) ) * 180 / M_PI;
       
 if constexpr ( Config::N_SCANS == 16 ) {
  if ( angle >= -15 && angle <= 15 ) {
   scan_idx = static_cast<int>( ( angle + 15 ) / 2 + 0.5 );
  }
 }

这里可以根据激光点所在线数对其进行一个分类，即每个激光扫描线上的点放在一个容器里，一共16个。还需要对每一个点建立一个索引，方便后续可以直接根据该索引找到这个点，对应代码为：

if ( scan_idx > -1 && scan_idx < Config::N_SCANS ) {
  scans_row_data_vec[scan_idx].push_back( pt );   
  point_index_vec[scan_idx].push_back( i );
 }

计算每个点的曲率

上面说了要计算曲率必须先找到集合 S ，我们在3.1步骤中确定每个点所在的线数并将其加入到对应的容器中就是为了给这个做准备的。因为在loam系列的方法中，集合 S 是在每个点所在扫描线上去找该点前后各5个点的。这很大程度上简化了算法过程，因为不用考虑激光点上下两个方向上的点了。

for ( size_t i = Config::Row_Index_Start; i < Config::N_SCANS - Config::Row_Index_End; i ++ ) {
 scans_row_curv_vec[i].resize( scans_row_data_vec[i].size() );
 for( size_t j = 5; j < scans_row_data_vec[i].size() - 5; j ++ ) {
  typename CloudType::PointType::ValueType diff_x = scans_row_data_vec[i][j - 5].x + scans_row_data_vec[i][j - 4].x + scans_row_data_vec[i][j - 3].x + scans_row_data_vec[i][j - 2].x + scans_row_data_vec[i][j - 1].x - 10 * scans_row_data_vec[i][j].x + scans_row_data_vec[i][j + 1].x + scans_row_data_vec[i][j + 2].x + scans_row_data_vec[i][j + 3].x + scans_row_data_vec[i][j + 4].x + scans_row_data_vec[i][j + 5].x;

  typename CloudType::PointType::ValueType diff_y = scans_row_data_vec[i][j - 5].y + scans_row_data_vec[i][j - 4].y + scans_row_data_vec[i][j - 3].y + scans_row_data_vec[i][j - 2].y + scans_row_data_vec[i][j - 1].y - 10 * scans_row_data_vec[i][j].y + scans_row_data_vec[i][j + 1].y + scans_row_data_vec[i][j + 2].y + scans_row_data_vec[i][j + 3].y + scans_row_data_vec[i][j + 4].y + scans_row_data_vec[i][j + 5].y;

  typename CloudType::PointType::ValueType diff_z = scans_row_data_vec[i][j - 5].z + scans_row_data_vec[i][j - 4].z + scans_row_data_vec[i][j - 3].z + scans_row_data_vec[i][j - 2].z + scans_row_data_vec[i][j - 1].z - 10 * scans_row_data_vec[i][j].z + scans_row_data_vec[i][j + 1].z + scans_row_data_vec[i][j + 2].z + scans_row_data_vec[i][j + 3].z + scans_row_data_vec[i][j + 4].z + scans_row_data_vec[i][j + 5].z;
   
  scans_row_curv_vec[i][j] = diff_x * diff_x + diff_y * diff_y + diff_z * diff_z;
 }   
}

每个点的曲率放在容器里，供后续使用。

根据曲率来筛选特征点

在步骤3.2中已经将每个点的曲率保存在了容器scans_row_curv_vec中了。这时候只需要遍历所有的点，并对每一个点的曲率根据阈值进行判断即可。

for ( size_t i = Config::Row_Index_Start; i < Config::N_SCANS - Config::Row_Index_End; i ++ ) {
 int j_start_index = 0;
 for ( size_t j = 0; j < scans_row_data_vec[i].size(); j ++ ) {
  if ( j >= j_start_index ) {
   // plane feature
   if ( scans_row_curv_vec[i][j] < 0.05 ) {
    output_cloud1_refer->points.push_back( input_cloud.points[ point_index_vec[i][j] ] ); 
   }
   // corner feature
   else if( scans_row_curv_vec[i][j] > 2.0 ) {
    output_cloud2_refer->points.push_back( input_cloud.points[ point_index_vec[i][j] ] );
   }

   //j_start_index = j + window_interval;
   j_start_index = j + 10;
  }
 }
}

注意这里为了降低特征点的数量，每隔若干点才判断一下，即语句j_start_index = j + 10的作用。

另外在loam系列方法中根据曲率判断特征点的方法比这里复杂一些，它要对点的曲率进行排序，选取曲率最高的若干个点作为角点，曲率最低的若干个点作为平面点。