激光SLAM论文简单导读--LOAM、VLOAM、LeGO-LOAM、LIO-SAM、LVI-SAM、LIMO、LIC-FUSION、TVL-SLAM、R2LIVE、R3LIVE

本博客以简述为主，不打算细讲论文细节，给出了相关详细导读的链接，欢迎沟通交流。

时间线

开篇巨作LOAM （张辑，RSS 2014， CMU）

LOAM是该方向的开山鼻祖，其主要的贡献在于使用前端里程计和后端建图优化两个线程（可能借鉴了视觉SLAM思想？），实现一个高精度激光SLAM系统， 该系统实际实验中采用的硬件是一个平面激光雷达加一个旋转轴，也取得优异的结果。

困难点：激光数据接收时间不同，运动导致的畸变很明显严重， 不利于后续的定位建图。
解决办法：采用两个线程（前端frame-frame里程计 + 后端frame-map优化）进行定位建图任务。

算法框图

论文对于激光雷达数据，定义了两个不同的术语：scan和sweep，依据论文，个人理解为前者为平面激光雷达绕本体旋转轴一周形成的点云，频率为10hz，该频率和平面激光雷达的旋转频率是一致的， 后者则是指激光雷达绕外面的轴旋转一个周期（180度）的点云，为1hz，相当于10scans的合成（而不是现在常用的那种昂贵的3D激光雷达）。流程图中，$\hat{p}$代表一个scan的点云，$p_k$代表一个sweep的点云。每一个scan结束都会进行一次Lidar Odometry， 每一次sweep结束才进行一次Lidar Mapping。

LIDAR ODOMETRY

特征提取： 设计下列公式计算一个度量值，用于判断每个点属于平面点或者线段点。注意该过程从每个独立的scan中提取特征（原因可能是由于传感器限制）。

在该度量中，c值较大的为edge point， c值较小的为planar point。此外还有一些工程策略，使得选择的特征分布均匀且可靠。

特征数据关联：采用frame-frame的思想寻找当前帧相对上一帧的匹配， 注意当前帧是随着时间在增长的，采用最近匹配方法寻找点-线和点-面匹配，最后通过最小化他们之间的距离来优化位姿。

整个里程计端的算法如下：

LIDAR MAPPING

后端mapping采用sweep-local map的方式进行，当前端一个sweep结束时，即启动该过程。
在k+1 sweep结束时，里程计模块生成了一个去畸变后的点云${\bar{p}}_{k+1}$, 该sweep [$t_{k+1}$, $t_{k+2}$]内的位姿变换$T_{k+1}^L$,mapping阶段将该点云${\bar{p}}_{k+1}$匹配和注册到世界系下。

Mapping阶段的匹配和位姿估计流程和Odometry阶段类似，但做了一些参数上的适应和工程上的优化。这一阶段在当前sweep特征提取的数量是Odometry阶段的10倍，然后根据位姿变换将其投影到世界系，再利用最近匹配方法寻找匹配，不同的地方在于， 寻找匹配线和面时是采用先求点云周围协方差，然后进行特征值分解的方法。对于边缘点，特征值的某一个值会远远大于另外两个值；而对于面点，两个特征值会远远大于另一个值。最后也是通过最小化点-线、点-面距离来估计位姿。

思考

• 为何如此提取点线特征？点线特征在整个位姿估计中的作用分别是？论文缺乏对应的消融实验（包括特征提取涉及到的工程化手段带来的好处没有实验支撑）
• 采用现在的3D激光雷达以后，这种特征提取方法是否可以进行优化？

激光视觉里程计VLOAM（张辑，ICRA2015， CMU）

算法简介：先利用视觉里程计估计位姿，并将雷达点云数据进行高频率低精度注册，然后基于雷达里程计的scan匹配同时优化运动估计和点云注册。相较LOAM而言，利用更高频率的视觉里程计提高点云去畸变和匹配的效果，进而提高算法精度。位姿估计过程并没用到视觉特征，是一个典型的视觉辅助激光的方案。算法的流程图如下：

视觉里程计

视觉里程计模块，利用视觉特征做帧-帧之间的快速相对位姿估计，从雷达数据中获取部分视觉特征点的深度值。

雷达里程计

雷达里程计由两阶段组成： sweep-sweep优化运动估计 + sweep-map在地图上匹配和注册当前sweep。该过程与LOAM的很类似，可看作是LOAM的odometry和mapping的结合，不同之处在于操作对象都是sweep，而不是scan。最后结合视觉里程计，可以输出与视觉相同频率的位姿。

从整体来看，视觉的作用主要是用于点云去畸变和提供后续点云匹配时的初始位姿，而最终的位姿估计并没用上视觉信息，在KITTI上的表现比LOAM好一丢丢。

激光-视觉-惯性里程计（张辑，JFR2018， CMU）

常见的激光视觉惯性里程计采用卡尔曼滤波或者因子图优化的方法，但作者观察到，本体位姿估计的漂移频率远远低于模块自身的运行频率，因此采用从粗到细的流水线pipeline，该方法采用一种模块化的数据处理流程用于处理图像、imu和雷达数据，并通过多层次的优化来进行运动估计和建图， 该流程如下图所示

主要贡献如下：

• We propose a modularized data processing pipeline to leverage range, vision, and inertial sensing for motion estimation and mapping through mulilayer optimization. Therefore, it achieves high accuracy and low drift.提出一种模块化的数据处理流程，利用雷达、视觉和惯性数据进行基于多层次优化的运动估计和建图；
• The proposed pipeline is dynamically reconfigurable. It fully or partially bypasses failure modules and combines the rest of the pipeline to handle sensor degradation. Therefore, it can handle environmental degradation and aggressive motion. 所提方法具有动态自适应，可以应对传感器退化的情况；
• The proposed pipeline employs a two‐level voxel representation and a multithread processing implementation to accelerate scan matching.所提流程采用两层次的体素表征方法来描述地图，并采用多线程加速匹配；
• The proposed pipeline is extendable to localization on an existing map and therefore can enable collaborative mapping, for example, between ground and air。所提流程被扩展到基于给定地图的定位，因此也可用于协同建图。
  这个算法可分为四个子系统：IMU预测子系统， VIO子系统和雷达匹配子系统。

IMU预测子系统

这一部分主要涉及相机系下（此处与常用的IMU系下的测量模型不同）的IMU测量模型和bias更新。

VIO子系统

VIO子系统主要借鉴作者之前的工作《Real‐time depth enhanced monocular odometry》，流程如下：

该系统是一个基于关键帧和特征点的VIO，不同之处在于只估计位姿，不估计特征点位置，这是因为从雷达获取的深度值足够准确。

lidar 匹配子系统

该子系统主要参考了作者之前的工作LOAM，参考其中的sweep-map匹配过程，整个子系统的流程如下：

此外还利用voxel对Map进行存储和多线程技术，以加速该匹配过程。

传感器退化

本方法可以有效应对传感器退化问题

基于给定地图的定位

整体流程与里程计流程一致，不同点在于雷达匹配阶段匹配的是给定地图，而且是一段时间（2s）内累积的lidar scan与给定地图进行匹配（降低算力考虑）。

结果对比

其中One-step方法的pipeline如下：

LeGO-LOAM：利用地面和平面聚类进行优化（Tixiao Shan， 2018， Stevens Institute of Technology）

LOAM的限制：计算资源较大， 低算力平台难以满足，因为它需要计算稠密三维点云的每一个点特征；当应用与地面无人车时，地面及其附近元素引起的噪声将一直影响算法。
解决办法：针对地面附近元素带来的干扰，提出使用地面分割的方法以提高对地面附近不稳定元素的适应性；针对算力问题，提出使用两阶段优化法来估计位姿：首先利用地面来估计[$t_z$, ${\theta }_{roll}$, ${\theta }_{pitch}$]，然后利用剩下部分的边缘点元素来估计其余的[$t_x$, $t_y$, ${\theta }_{yaw}$]，使得整个系统可以在嵌入式平台上面实时运行。

整个算法流程框图如下：

分割与特征提取

采用传统的几何和图像处理方法对激光点云进行分割，以提取地面。特征提取与LOAM的一致，最终只保留使用属于地面部分的平面点和其他部分的边缘点，点云示意如下：

Lidar Odometry

与LOAM的机制一样，采用帧-帧之间的点-线和点-面匹配来估计雷达里程计。不同之处在于，该方法只对属于地面的平面点和非地面点的边缘点进行匹配，且在匹配过程中利用了分割的类别信息以筛出错误的匹配。然后采用两阶段的L-M优化法来估计位姿：首先最小化地平面点到地面的距离来估计[$t_z$, ${\theta }_{roll}$, ${\theta }_{pitch}$]，然后利用剩下部分的边缘点元素来估计其余的[$t_x$, $t_y$, ${\theta }_{yaw}$]。两步优化法可在算力减小35%的情况下获得同样的精度。

Lidar Mapping

该阶段与LOAM的一致，主要不同处在于地图的存储方式：LOAM存储单个点云地图，本方法则存储单个的特征集合。同时改方法还可在回环后使用Pose Graph以纠正漂移。

紧耦合IMU的激光惯性里程计–LIO SAM （Tixiao Shan， 2020，MIT）

LOAM的局限：难以回环，难以融合其他传感器，特征丰富区域时难以在线优化，大范围场景中会有漂移。为此提出一种采用因子图+滑窗+紧耦合lidar融合IMU的方法，该融合思路与VINS 和相似。
整个因子图中包含四种因子约束：lidar odometry、IMU预积分、GPS和回环约束。

IMU预积分约束

该约束和VINS中的一样，具体可参考On-Manifold Preintegration for Real-Time Visual-Inertial Odometry 和 VINS

雷达里程计约束

采用关键帧思想，每隔一段距离或角度则选择一个关键帧。在关键帧中提取平面和边缘线的点特征，历史最新的n帧合成sub-keyframes,将其中的特征变换到世界系下形成local voxel map。 特征的匹配采用LOAM的紧邻匹配方法，将帧经过变换与其之前的local voxel map进行匹配，该帧的初始位姿由IMU积分得到

GPS因子和回环因子

GPS因子可以提供一个全局绝对约束，可减少漂移， 回环因子则与视觉中的回环基本一样， 实验表明回环可以极大地改进高度方向的估计漂移问题。

LVI-SAM： 在LIO-SAM的基础上融合视觉（Tixiao Shan， 2021，MIT）

该方法是LIS与VIS的集成，且两者之间又互相促进，其中VIS为LIS提供初始位姿估计，支持后者的匹配，LIS则为VIS提供更好的初始化和特征深度值，利用VIS做回环检测，利用LIS进行之后的回环校验。这两个系统只要有一个正常，整个系统就可以正常运行，展示了很好的系统鲁棒性。

VIS系统借鉴使用了VINS-MONO，但利用了Lidar数据辅助初始化，并给特征值赋予深度值。
LIS系统则利用LIO-SAM。
简言之，LVI-SAM = VINS-Mono + LIO-SAM + 视觉与激光之间的辅助。

LIC-FUSION: 基于MSCKF框架的雷达-惯性-视觉融合 (左星星， 2019， 浙大)

主要贡献：提出一种在MSCKF框架上的雷达-惯性-视觉里程计，同时具有传感器外参标定功能。熟悉MSCKF的就懂，下面会涉及一大堆的数学推倒。

系统介绍

A，系统状态量


系统状态量包括IMU系的位姿、速度和bias；相机、激光雷达的外参；相机、雷达的位姿。

B，IMU递推（运动学）

该部分是常用的IMU运动学

C，状态增广
每当收到新的图像或者雷达数据，IMU就会根据运动学对系统状态进行推算，该过程既是EKF中的状态预测，获得新时刻下的状态及其协方差矩阵。

D， 测量模型

• 雷达特征测量
  参考LOAM的思路，在雷达帧上提取线和面上的点特征，采用最近了匹配的方式实现特征跟踪，可得到线上的点特征残差（点-线距离）为：
  
  上述残差在推算位姿处线性化：
  
  为了进行状态更新， 需要知道上述残差的协方差$C_r$:
  
  该协方差矩阵取决于传感器测量噪声。对于面上的点特征，可采用相同的流程处理。

• 视觉特征测量
  与MSCKF的一致， 提取FAST交点， 光流跟踪， 残差为重投影误差， 采用零空间投影，只估计sliding window内的位姿，不估计特征点位置。

E, 测量压缩
残差在新位姿处线性化后的方程可提炼为

为降低计算量，对线性化矩阵$H_X$进行QR分解：


有了上述协方差矩阵、预测位姿和协方差和线性化后的残差等，即可采用标准的EKF流程更新得到新的状态及协方差。

LIC-FUSION2.0：在LIC-FUSION的基础上添加滑窗和平面跟踪算法（左星星，IROS2020,浙大/ETH）

参考师兄大佬的论文导读
主要内容是在MSCKF和OpenVINS框架中加入激光测量信息，借鉴了视觉SLAM的pipeline，提出了一种新的激光-惯性-视觉里程计：LIC-Fusion2。它的前身是LIC-Fusion，在此基础上加入了如下特性：

• 将激光平面特征的跟踪从相邻两帧扩展到滑动窗口中的多帧中，并提出一种先进的用于改进平面跟踪的外点去除方法;
• 在状态变量中加入激光平面特征。同时估计滑窗中的传感器位姿，视觉特征点和激光平面。

该方法采用LOAM的方法提取平面点，并用平面点拟合得到平面，该版本并没用到LOAM中的边点特征，但在future work中提到会研究基于边缘特征的跟踪。

LIMO:激光单目视觉里程计（Johannes Graeter，Karlsruhe Institute of Technology，IROS2018）

详情的论文导读可以参考师兄的博客【论文阅读41】LIMO: LiDAR-Monocular Visual Odometry

本方法是一个典型的激光辅助视觉的SLAM方法，主要贡献思想是提出了一种基于LiDAR点云的视觉特征点深度估计方法，如下流程所示，该方法被用于后续的多种视觉-雷达融合方案所借鉴。同时针对关键帧选择，特征点选择等工程优化方面提出许多策略。思路简单，但取得的效果很好。
从雷达数据中获取特征点深度距离的方法：
1）首先寻找该特征点周围的矩形框内的激光点。
2）然后对这些激光点按照深度进行划分。
3）寻找最靠近该特征点的深度区间的点云，拟合平面。
4）我们认为该特征点位于该平面上，根据光心和特征点的连线与平面的交点记为该特征点的深度。
5）检测估计深度的准确性：光心和特征点连线与平面的夹角必须小于某个阈值；拒绝深度高于30m的特征点以防外点。

PL-LVO：基于点线特征的激光视觉里程计（Shi-Sheng Huang，IROS2020，清华）

利用点线特征的激光雷达-视觉里程计，详细论文解读可参考https://zhuanlan.zhihu.com/p/153264331

该方法是一个典型的激光辅助视觉的里程计方案，从视觉图像中提取点线特征，而后从激光数据中获得点线的深度距离信息，用于恢复尺度和执行后续的ICP位姿估计。

R2LIVE：激光-视觉-IMU紧耦合的状态估计器（lin jiarong， 港大， 2021）

主要贡献：

• 我们以紧耦合的方式融合了激光雷达、惯导、视觉的数据，实验证明我们的方法可以鲁邦处理包括剧烈运动、传感器退化、甚至是有很多移动物体的隧道场景，包括激光雷达只有很小FoV的场景。
• 我们提出的框架包括一个高频运行的基于滤波方法的里程计模块，和一个低频运行的基于因子图的优化模块。里程计模块以一个kalman滤波器迭代融合雷达、惯导、视觉传感器，取得了实时的状态估计。因子图优化模块进一步优化滑窗内的各传感器位姿和视觉地图点位置。

流程图：

该方法由一个基于误差的迭代卡尔曼滤波器和一个基于因子图的图优化模块组成。

迭代卡尔曼滤波器采用标准的卡尔曼流程，IMU作为预测，激光雷达或者视觉作为观测。滑窗优化则借鉴了VINS-Mono的方法，该因子图如下图所示，其中雷达帧位姿和和激光雷达特征在滑窗中保持不变。

R3LIVE：雷达惯性和视觉惯性里程计的融合（lin jiarong， 港大， 2022）

该方法包含两个子系统：激光雷达-惯性里程计 (LIO) 和视觉-惯性里程计 (VIO)。其中LIO (FAST-LIO) 利用 LiDAR和惯性传感器的测量数据构建全局地图几何结构（3D 点云位置）；VIO 利用视觉-惯性传感器的数据来渲染地图纹理（3D 点云颜色），同时VIO 还通过最小化帧到地图的光度误差来直接、有效地融合视觉数据。
主要贡献：

• 提出了一个融合框架，包含定位建图和上色，框架里包含一个LIO模块和一个VIO模块，整个系统可以实时的重建密集3D彩色环境。
• 基于彩色RGB点云提出了一个新的VIO系统，通过最小化三维地图点颜色和该三维点在当前图像帧中的颜色的光度误差，避免了提取突出的视觉特征点的需求，提升了运行速度，并且使得这个VIO对于无纹理环境更加鲁棒。
  从系统处理流程来看，该方案是一个松耦合融合激光雷达和视觉的方法。
  

SVIL-SLAM：双目视觉惯性雷达SLAM（Weizhao Shao， CMU，IROS2019）

详细论文解读可参考：【论文阅读16】Stereo Visual Inertial LiDAR SLAM

双目视觉里程计采用的是，采用光流跟踪、滑窗和边缘化思想，类似双目VINS。

雷达建图部分有借鉴LOAM的思路，通过最小化点-线和点-面特征来进行基于雷达的定位建图，使用高频的VIO位姿提供初始位姿进行帧图匹配和点云去畸变。同时，雷达数据被用于回环时提供ICP匹配来提高回环检测的准确率。

TVL-SLAM：紧耦合视觉-雷达SLAM

不同于之前的视觉雷达slam，将视觉的结果作为雷达的初始值，或者雷达给视觉提供深度测量值，本方法采用BA的方法在后端同时优化视觉残差和雷达残差。为了实现大规模的BA优化，本方法借鉴使用ORB_SLAM2和LOAM，关注后端准确而有效的雷达残差压缩。本方法主要贡献如下：

• 视觉和雷达测量都被用于运动估计，利用视觉和雷达运动的交叉校验来去除外点，克服视觉或者雷达退化的情况，在但传感器退化的情况下获得了相较独立视觉或者雷达方法更高的精度。
• 视觉和雷达的回环检测在闭环阶段被联合使用以确保高精度。为了提升计算效率，我们提出通用雷达因子（GLF）以将多雷达残差压缩成六维残差；
• 视觉-雷达融合框架被拓展成具有标定功能的SLAM，以缓解不准确的外参带来的不利影响；
• 在KITTI上取得非常好的效果。

融合Lidar后的改进：

• 视觉位姿跟踪后，计算雷达的scan-map注册，并通过视觉-雷达紧耦合来进一步优化位姿；
• 在局部和全局BA时加入额外的雷达残差和雷达子地图原点位姿
• 在视觉回环检测之外新增额外的基于雷达的回环检测模块；
• 为了更好地评估SLAM精度，SLAM流程被修改成单线程和阻塞式；

激光与视觉之间的辅助

• 激光给视觉提供特征深度，如LIMO，PL-LVO
• 视觉提供高频位姿初值和辅助激光去畸变，如VLOAM，SVIL-SLAM
• 因子图中两种特征独立共存，提高算法鲁棒性，如LVI-SAM，R3LIVE，TVL-SLAM，R2LIVE，LIC-FUSION
• 利用视觉-激光对场景进行重建，如R3LIVE
• 视觉利用3D激光地图进行定位， 如Stereo Camera Localization in 3D LiDAR Maps
• 视觉-激光联合闭环检测：LVI-SAM，SVIL-SLAM
• 特征层次的融合： 暂无，也可能成为研究方向点

融合方法

EKF：R3LIVE， LVIO， LIC-FUSION
因子图优化：TVL-SLAM， LVI-SAM
EKF+因子图优化：R2LIVE