开源3D激光（视觉）SLAM算法汇总（持续更新）

目录

一、Cartographer

二、hdl_graph_slam

三、LOAM

四、LeGO-LOAM

五、LIO-SAM

六、livox-loam

七、Livox-Mapping

八、FAST-LIO2

九、LVI-SAM

十、FAST-Livo

十一、R3LIVE

---

一、Cartographer

Cartographer是由谷歌于2016年开源的一个支持ROS的室内SLAM库，并在截至目前为止，仍然处于不断的更新维护之中。

1. 代码极为工程，多态、继承、层层封装的十分完善。提供了方便的接口，便于接入IMU、（单/多线）雷达、里程计、甚至为二维码辅助等视觉识别方式也预留了接口（Landmark）。根据其代码的结构设计来看，显然是由一个大型的研发团队集中研发，而非少数科研人员的demo型代码。

2. Cartographer支持2D和3D激光雷达的输入，实现机器人定位，并构建栅格地图。2D-SLAM：基于2D栅格地图，可以直接用于导航。3D-SLAM：基于hybridGrid，译为混合概率地图，我理解为3D栅格地图。 该地图难以直接使用。

3. 其主要用到的库：a.线性代数库Eigen3，b.用来读取配置文件中的参数库Lua，c.谷歌开源的非线性最小二乘优化库Ceres，d.Google开源的很流行的跨平台通信库Protobuf

4. 分为Cartographer和Cartographer_ros, 后者为前者基于ros的接口，换言之，Ros在其中的作用仅为面向用户，算法本身不以ros传输数据，而是以Protobuf进行各个进程之间的数据编码，再进行通信。

5. Cartographer的地图（map）以子地图（submap）的形式组成。分为前端和后端。 前端：根据帧间匹配算法（scan-match），实时根据激光(scan)来推测累积的scan相对于submap的位姿。 后端：检测回环（发现在已到达的位置附近），修正各个submap之间的位姿。

二、hdl_graph_slam

hdl_graph_slam同Cartographer一样，能同时融合gps、imu和lidar三种传感器，不过相比Cartographer来说，它在资源消耗、代码复杂度和建图流程方面要精简不少。

1. 刚才提到代码架构简单，且调用通用的Eigen和g2o进行核心的运算，适合对整个SLAM架构的学习。

2. 主要包括平面检测及提取、点云预处理、激光里程计、闭环检测、后端优化等流程。

3. 平面检测和平面约束是HDL的一大特色。平面拟合采用的是PCL的RANSAC，比较简单，换用其他方法效果可能会更好，可参考我的另一篇文章

《超详细的激光点云地面分割（可行驶区域提取）方案_zllz0907的博客-CSDN博客》。另外平面约束，HDL强假设建图的场景为平面，将建图初始平面作为固定平面，之后的平面与其误差加入到约束当中。因此HDL不适合有坡道的场景建图。

４.　激光里程计采用当前激光帧与上一时刻的点云进行匹配，调用PCL，支持ICP、GICP、NDT。

５.　后端融合采用g2o进行优化，关键帧作为顶点，平面约束、gps、imu、闭环等作为边进行约束。如下图，其中imu约束的是姿态角和重力加速度方向。

 

三、LOAM

LOAM为Zhang Ji于2014年在RSS期刊发表的关于三维激光传感器的SLAM算法。 它和Cartographer完全不是同一个思路。

1. Cartographer主要解决室内问题，LOAM室内外都可以，但是没有回环检测

2. Cartographer的3D部分，更像是2D的扩展：即用2D的思路去做3D的事情。而LOAM则主要解决3D问题，其核心思路难以解决2D问题。

3. 提出了新颖的特征提取方式，角点和面点。

4. 根据时间戳，对旋转的雷达采集时间不一致进行运动补偿。

5. 融合了scan to scan（里程计部分） map to map（建图部分）的思想。

6. 没有回环检测。（发表时间较早，比谷歌Cartographer要早两年）

7. 不能处理大规模的旋转变换。

四、LeGO-LOAM

Lego-LOAM是TiXiao Shan发表在IROS2018的文章，其是以LOAM为框架衍生出的新算法，主要在于两点提升：轻量级 和 地面优化。

Segmentation：对点云进行分割，分割为地面和非地面区域；

Feature Extraction：基于分割后的点，和LOAM类似的算法提取出边缘点和平面点；

Lidar Odometry：基于提取的特征点，scan-to-scan推算两帧激光之间的相对位姿变换（使用两次LM优化），频率较高（10Hz）；

Lidar Mapping： scan-to-map，构建全局地图，获得世界坐标系下的位姿，频率较低（2Hz）；

Transform Intergration：与LOAM相同，实时利用世界坐标系下的位姿和两个时刻之间的相对位姿，更新各个时刻世界坐标系下的位姿。

1. LOAM缺点：

没有回环检测 ；计算时间复杂度较高，基于三维空间中的位姿进行优化；

户外可能受到各种噪声影响，例如树上摇晃的树叶，地上的杂草。而这些点未必会重复出现在前后两帧激光中。而错误的特征点将会影响位姿精度。

LOAM需要提取平面点和边缘点，由于车体上下颠簸，竖直维度提取的平面点很容易造成误差。

2. 因此Lego-LOAM的贡献：1.着重于解决一些非城市化道路或非平整道路上LOAM存在的问题。2.轻量化，改进算法，使其在TX2上也可以实时运行。

3. Lego-LOAM有一个显著的缺陷——依赖地面。如果用无人机，那么就难以确定地面了。 当然论文作者提到，对于无人机，则不提取地面点，直接就像LOAM中那样正常提取边缘点和平面点。但是我认为这样算法的核心优势就丢掉了。

五、LIO-SAM

LIO-SAM是TixiaoShan在2020年IROS发表的Lego-LOAM续作。实际上也是Lego-LOAM 的扩展版本，添加了IMU预积分因子和GPS因子。

1. 使用关键帧Keyframe，使用关键帧进行匹配，丢掉了关键帧之间的帧。（阈值设置为1m和10度）

2. 四种因子：

   IMU预积分因子；

   雷达里程计因子，激光“关键帧” 和 “之前的N个关键帧构成的体素地图” 进行匹配；

   GPS因子： 当估计位姿的方差大于GPS位置方差时加入；

   回环检测因子，由关键帧和候选关键帧相邻的2m+1个关键帧帧图匹配得到。

3. 该方法使用多传感器融合的方法，利用因子图优化，计算位姿；

4. 回环检测部分基本和Lego-LOAM的特点一致，即不具备重定位能力（该方法前提是需要知道自己的大致位置，和历史中附近的位置进行匹配。

六、livox-loam

Livox-loam为香港大学于2019年开源的工作，其主要由两篇论文组成：

1. Loam livox: A fast, robust, high-precision LiDAR odometry and mapping package for LiDARs of small FoV 这篇主要讲算法前端和后端，针对于LOAM的改进方法；

2. A fast, complete, point cloud based loop closure for LiDAR odometry and mapping 这篇主要讲算法使用的回环检测方法；两篇文章共享同一个github开源链接。

livox-loam依然采用和LOAM一样的方式，选取边缘点和平面点。但是它和LOAM最大区别就是，在选取候选的特征点时，变得更加“小心”了。

四大原则：

1. 接近视角边缘的点不要。说白了就是射线和x轴夹角大于17度的就不选。

2. 反射强度太大或者太小的点不要。

3. 和平面夹角很小的点不要

4. 部分被遮挡的点不要

位姿的迭代估计：

计算位姿的目标函数，与LOAM中的一致，为点到线的距离（从边缘点中选点），点到面的距离（从平面点中选点），也采用协方差矩阵特征值的方式，提取边缘线和平面。

外点和动态点的过滤：

算法采用一种比较朴素的方式过滤外点和动态点，先用特征直接匹配一下，计算出相对位姿，然后投影点云；然后比较投影后的点云与地图，去除掉太大的点，当成外点（outliers）或动态点（dynamic objects）。然后用剩下的点做继续匹配，进行优化迭代。

回环检测：

Livox-loam的回环检测部分其提取的特征，是基于2D直方图。

七、Livox-Mapping

完整的一套基于固态激光雷达的SLAM系统，开源了建图、定位及相应的docker环境。也是支持lidar、imu和GPS。

1. 前端基于开源的LIO-Livox开发，后端基于ScanContext做的闭环检测。

2. 固态激光雷达由于FoV比较小，所以容易出现Z轴漂移。因此这套方案也加入了地面约束。具体方法其实与hdl_graph_slam类似。

3. 支持多个rosbag包建图。首先多个包统一坐标系，同时在后端维护了一个长期优化的关键帧序列，利用scancontext及icp的方法实现不同关键帧之间的回环和优化。

4.  兼容inetractive_slam，即可使用其进行地图编辑。同时提供了一项使用图像数据，借助mmdetection和lidar点投影进行动态物体去除。具体没试过，不知道去除效果怎么样，后面也会写相关文章介绍如何进行动态点去除，敬请关注。

八、FAST-LIO2

FAST-LIO2是一种快速、鲁棒的、通用的激光雷达惯性里程计框架，FASTLIO2以高效紧耦合的方式迭代卡尔曼滤波器为基础，具有两个关键的新颖之处，可实现快速、稳健和精确的激光雷达的建图和导航。

1. 直接将原始点云配准到地图（随后更新地图），而不提取特征，这样可以利用环境中的细微特征，从而提高准确性，这里不采用人工设计的特征提取模块，使其自然适应不同扫描模式的新兴激光雷达；

2. 通过增量k-d树数据结构kd-tree来维护地图，该结构支持增量更新（即点插入、删除）和动态平衡。与现有的动态数据结构（八叉树，R-树，nanoflann k-d树），kd树实现了卓越的整体性能，同时自然支持下采样，我们对来自各种开放式激光雷达数据集的19个序列进行了详尽的基准比较。FAST-LIO2比其他最先进的激光雷达惯性导航系统低得多的计算量实现了持续更高的精度，在小视场的固态激光雷达上也进行了各种真实世环境的实验，总体而言，FAST-LIO2计算效率高，鲁棒性强，多功能，同时仍能实现比现有方法更高的精度，FAST-LIO2和数据结构kd树的实现都是在Github上开源了。

九、LVI-SAM

LVI-SAM为Lego-LOAM和LIO-SAM作者Tixiao Shan的最新工作，发表在ICRA 2021上。 提出了一个基于图优化的多传感器融合框架，具有多个子系统： 视觉惯性子系统（VIS） 和 雷达惯性子系统 （LIS）； 单目+雷达+imu融合鲁棒性：任一子系统失效，不会导致整个系统挂掉。

现有方法的不足：

1. 基于激光的方法，在结构比较简单的环境里，容易失效——因为特征只有一个简单的“距离”信息；

2. 基于视觉的方法，容易受到光照改变、快速运动导致的图像模糊等问题； 所以，一般会加入IMU传感器——但IMU具有bias，其估计的并不是很准。

LVI-SAM使用了两个子系统，视觉惯性系统（VIS）和雷达惯性系统（LIS）： 

1. 视觉惯性系统用雷达惯性系统来初始化；用LIS计算的位姿，作为VIS的初始优化值；

2. VIS中需要根据图像计算特征的深度，可以利用雷达的测量数据来辅助优化；

3. LIS计算两个点云位姿变换的优化初值，同样也可以用VIS的视觉估计来计算；

4. 回环检测可以用视觉信息确定初值，再用雷达数据优化。

十、FAST-Livo

香港大学火星实验室最新的成果,一种融合LiDAR-IMU-视觉的里程计系统，它由两个紧耦合的子系统构成：VIO子系统和LIO子系统。

本文的贡献如下：

1. 一种紧耦合的LiDAR-inertial-visual里程仪框架，它建立在两个紧耦合的里程计系统之上：LIO子系统和VIO子系统，这两个子系统都不需要提取特征，通过将各自的激光雷达或视觉数据与IMU进行融合来联合估计系统状态。

2. 一种直接而有效的VIO子系统，它重用了LIO子系统的点云地图。具体地说，通过最小化光度误差，将地图中的点投影到新一帧图像上来对齐位姿。LIO子系统的点云地图在VIO子系统中的重用避免了视觉特征的提取、三角化，将两个传感器紧耦合在一起。

3. 将所提出的系统代码开源，该代码可以在Intel和ARM处理器上实时运行，并且既支持旋转雷达，也支持固态雷达。

4. 分别在开放数据集和自己的设备数据上进行了实验。结果表明，我们的系统性能优于其他同类系统，并且能够以较低的计算量工作在传感器退化的环境。

十一、R3LIVE

R3LIVE包含两个子系统，即激光雷达-惯性里程计（LIO）和视觉-惯性里程计（VIO）。LIO子系统（FAST-LIO）利用LiDAR和惯性传感器的测量结果构建全局地图（即3D点的位置）的几何结构。VIO子系统利用视觉-惯性传感器的数据来渲染地图的纹理（即3D点的颜色）。更具体地说，VIO子系统通过最小化帧到地图的光度误差来直接有效地融合视觉数据。所开发的系统R3LIVE是在我们之前的工作R2LIVE的基础上开发的，经过了精心的架构设计和发现。实验结果表明，所得到的系统在状态估计方面比现有系统具有更强的鲁棒性和更高的精度（可看附件视频）。

R3LIVE是一个面向各种可能应用的多功能且精心设计的系统，它不仅作为实时机器人应用的SLAM系统，还可以为测绘等应用重建密集、精确的RGB彩色3D地图。此外，为了使R3LIVE更具有可扩展性，我们开发了一系列用于重建和纹理化网格的离线实用程序，这进一步缩小了R3LIVE与各种3D应用程序（如模拟器、视频游戏等）之间的差距。为了分线我们的发现并为社区做出贡献，我们在GitHub上开放R3LIVE的源代码，包括我们所有的代码/软件实用程序和设备的机械设计。

1. 该论文提出了一个实时同步定位、建图和着色的框架。该框架由用于几何结构重建的激光雷达-惯性里程计（LIO）和用于纹理渲染的视觉-惯性里程计（VIO）。整个系统能够实时重建环境的稠密3D RGB点云图。

2. 该论文提出了一种基于RGB着色点云图的新型VIO系统。VIO通过最小化观察到的地图点的RGB颜色与其在当前图像中的测量颜色之间的光度误差来估计当前状态。这样的过程不需要环境中的显著视觉特征，，并且节省了相应的处理时间（如特征检测和提取），使该系统在无纹理环境中具有更强的鲁棒性。

3. 论文把所提出的方法实现为一个完整的系统R3LIVE，该系统能够实时且低飘移地构建稠密、精确的三维RGB着色环境点云图。且整个系统在各种室内和室外环境中进行了验证。结果表明，该系统在行驶1.5公里之后，平移仅飘移0.16米，旋转飘移仅3.9度。

4. 作者在GitHub上开源了整个系统。并且还开发了几种离线工具，用于从彩色点云重建和纹理化网格。而且设备的软件实用程序和机械设计也是开源的，以造福于可能的应用程序。

参考：

https://blog.csdn.net/zkk9527/article/details/116160508