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

原文连接

一、Cartographer

二、hdl_graph_slam

三、LOAM

四、LeGO-LOAM

五、LIO-SAM

六、S-LOAM

七、M-LOAM

八、livox-loam

九、Livox-Mapping

十、LIO-Livox

十一、FAST-LIO2

十二、LVI-SAM

十三、FAST-Livo

十四、R3LIVE

十五、ImMesh

十六、Point-LIO

一、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因子。

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

IMU预积分因子；

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

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

回环检测因子，由关键帧和候选关键帧相邻的2m+1个关键帧帧图匹配得到。
该方法使用多传感器融合的方法，利用因子图优化，计算位姿；
回环检测部分基本和Lego-LOAM的特点一致，即不具备重定位能力（该方法前提是需要知道自己的大致位置，和历史中附近的位置进行匹配。

六、S-LOAM

S-LOAM宾夕法尼亚大学GRASP实验室于2020年发表在RAL上的文章，旨在解决在森林环境中的SLAM问题。

本文描述了一种基于语义分割和LOAM的端到端树木直径估计方法，这类型环境由于地上和树都被叶子，荆棘和藤蔓植物的影响，构建准确的建图是一个有挑战性的问题。

1. 提出了一种基于位姿优化的语义特征，在估计机器人位姿的同时优化树木的模型。

2. 本方法利用一个定制的虚拟现实工具来标记3D扫描信息，用于训练语义分割网络。分割后的点云会被用来计算网格图，用于识别各个实例，并且提取相关特征用于SLAM模块。

3. 当传统的基于雷达和图像方法在无人机或者手持设备在森林场景中都失效时，该方法能够鲁棒，可拓展，并且能自动生成树木直径的估计。

七、M-LOAM

M-LOAM是港科大RAM实验室在2021年发表在IEEE Transactions on Robotics的文章，旨在通过结合多个激光雷达的输入，来提升机器人的感知范围。

本文提出了一个能够利用多激光雷达获得鲁棒与实时的外参标定、里程计与建图效果的系统。

1. 首先提取点云中的边缘与平面点。经过外参初始化后，使用一个基于滑窗的多激光雷达里程计来同时估计位姿与在线标定，其中包含了标定结果的收敛性判断。

2. 提出了一种建图算法，通过构建全局地图并结合足够多的观测，构建并尝试减少测量与估计的不确定性来优化位姿。

八、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最大区别就是，在选取候选的特征点时，变得更加“小心”了。

四大原则：

接近视角边缘的点不要。说白了就是射线和x轴夹角大于17度的就不选。
反射强度太大或者太小的点不要。
和平面夹角很小的点不要
部分被遮挡的点不要

位姿的迭代估计：

计算位姿的目标函数，与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点投影进行动态物体去除。具体没试过，不知道去除效果怎么样，后面也会写相关文章介绍如何进行动态点去除，敬请关注。

十、LIO-Livox

Livox官方推出的SLAM方案，旨在解决在高速场景SLAM时遇到的挑战。仅用单台Livox Horizon (含内置IMU) 实现高鲁棒性的激光-惯性里程计，可在各类极端场景下鲁棒运行，并达到高精度的定位和建图效果。

1. 系统由ScanRegistration和PoseEstimation两个模块组成。前者负责从原始点云中去除动态物体并提取特征点供位姿估计使用。后者包括两个线程，主线程进行IMU初始化和状态估计，另一个线程构建和管理特征地图。

2. 采用欧式聚类方法将原始点云聚类为地面点，背景点，和前景点。其中前景点被视为动态物体，并在特征提取过程中去除。去除之后，系统在动态场景中可保持很高的鲁棒性，即便激光雷达的大部分视场角被动态物体遮挡，也可以准确地定位和建图。

3. 特征点分为三类：边缘特征，平面特征，和不规则特征。首先将每条扫描线上的大曲率点和孤立点提取出来作为边缘特征。然后使用主成分分析法区分平面特征和不规则特征。对于不同的距离，特征提取使用不同的阈值，这样便可使能特征点在空间中的分布尽可能均匀。

4. 状态估计模块包含LO模式和LIO模式，如果IMU初始化成功了，系统将从LO模式切换到LIO模式，否则系统将继续运行LO模式并尝试进行初始化。系统可使用IMU预积分或者匀速运动模型来消除点云的自运动畸变。

5. IMU初始化过程参考ORB-SLAM3，采用最大后验概率估计的方法进行IMU初始化，一步联合估计IMU偏置，速度，以及重力方向。系统可在任意运动状态下（静止、运动或混合状态）完成初始化。在初始化完成之后，系统将执行一个基于滑动窗口的紧耦合传感器融合模块来优化窗口内的IMU位姿、速度和偏置。

十一、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融合鲁棒性：任一子系统失效，不会导致整个系统挂掉。

现有方法的不足：

基于激光的方法，在结构比较简单的环境里，容易失效——因为特征只有一个简单的“距离”信息；
基于视觉的方法，容易受到光照改变、快速运动导致的图像模糊等问题；所以，一般会加入IMU传感器——但IMU具有bias，其估计的并不是很准。

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

视觉惯性系统用雷达惯性系统来初始化；用LIS计算的位姿，作为VIS的初始优化值；
VIS中需要根据图像计算特征的深度，可以利用雷达的测量数据来辅助优化；
LIS计算两个点云位姿变换的优化初值，同样也可以用VIS的视觉估计来计算；
回环检测可以用视觉信息确定初值，再用雷达数据优化。

十三、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上开源了整个系统。并且还开发了几种离线工具，用于从彩色点云重建和纹理化网格。而且设备的软件实用程序和机械设计也是开源的，以造福于可能的应用程序。

十五、ImMesh

R3LIVE重建的彩色点云是要得到雷达在地图里面全环境的辐射信息，才能够重建出地图的纹理，且必须要求每个点覆盖到了才会有。然而，点云的密度有限，即不能在没有点的地方看着这个点，现有一种解决方案是重建地图的网格(mesh)模型，然后用相机得到图像RGB信息后贴到网格上，这样能够无损的去重建地图纹理。在这个背景下，做了一个在线激光雷达的实时定位和网格重建系统。主要贡献是完成了一个实时的、在线的网格重建。

1、提出了一种新颖的系统，可以在线估计传感器的姿态和重建周围环境的网格。它的本地化是建立在作者以前的工作VoxelMap的基础上的，它可以比其他同类方法(例如FAST-LIO2、SUMA、Mulls、Lego-Loam等)更高效、更准确地估计传感器的姿态。它的网格划分模块实现了一种新颖的网格重建方法，该方法以增量的方式高效地重建网格，并能在标准桌面CPU上实现大规模场景的实时性能。

2、在网格划分模块中实现了一种新的网格重建方法，该方法直接利用注册的LiDAR点作为网格顶点，在线重建网格三角形面片(即，三个三角点的索引)以递增的方式。具体地说，我们的网格化模块首先利用一种高效的层次化体素数据结构来快速找到新扫描中包含点的体素。然后，通过降维将体素三维网格问题转化为二维网格问题。最后，使用体素网格的Pull、Commit和Push步骤增量地重建三角形面片。

3、通过大量的实验对IMMesh的运行时间性能和网格划分精度进行了评估。首先通过提供实时视频演示来验证整体性能，演示如何在数据收集过程中立即重建网格。然后，使用在不同场景中使用不同类型的激光雷达收集的四个公共数据集对IMMesh进行了广泛的测试。最后，通过与现有基线的比较来评估IMMesh的运行时间性能和网格划分精度。

4、通过两个实际例子演示了实时网格如何应用于潜在的应用。首先证明了IMMesh可以应用于LiDAR点云增强，它可以以规则的方式输出增强的点，并且与原始的LiDAR扫描相比，具有更高的密度和更宽的视场。然后，将IMMesh和我们以前的工作R3LIVE结合起来，实现了场景的无损纹理重建。