LIDAR基础/点云处理/LOAM/LIO

1 LIDAR基础知识

走进自动驾驶传感器（一）——激光雷达
3D采集设备（一）激光雷达认知
系统组成和分类
激光雷达的7大分类讲解
LIDAR关键指标
测距方式、光源和扫描方式
三角测距法和TOF原理
ROS中LaserScan数据格式
知名激光雷达公司

2 点云及数据处理方法

2.1 点云基础知识

3D点云基础知识(一)-初始入门-知乎整理（一）

2.2 点云粗配准

粗配准是指在点云相对位姿完全未知的情况下对点云进行配准，找到一个可以让两块点云相对近似的旋转平移变换矩阵，进而将待配准点云数据转换到统一的坐标系内，可以为精配准提供良好的初始值。
常见粗配准算法：

• 基于特征匹配(PFH)的配准算法：
  SAC-IA 采样一致性初始配准算法（Sample Consensus Initial Alignment）PCL库已实现，基于FPFH
• 基于穷举搜索的配准算法：
  RANSAC
  4PCS

2.2.1 RANSAC算法

RANSAC (RAndom SAmple Consensus)算法最早是在数学/统计学领域提出，它是一种利用随机采集的样本来准确拟合出整体数学模型参数的方法。它的主要思想是从给定的样本集中随机选取一些样本并估计一个数学模型，将样本中的其余样本带入该数学模型中验证，如果有足够多的样本误差在给定范围内，则该数学模型最优，否则继续循环该步骤。
后来，RANSAC算法被引入三维点云配准领域，产生了基于RANSAC思想的RANSAC点云配准算法，其算法过程主要如下：

其本质就是不断的对源点云进行随机样本采样并求出对应的变换模型，接着对每一次随机变换模型进行测试，并不断循环该过程直到选出最优的变换模型作为最终结果。根据大数定律，可知道在进行大量重复采样实验的情况之下，随机模拟可以近似地将正确结果求解出来。 当然，RANSAC配准算法也存在着有限次随机性带来的不稳定配准和计算量大等弊端。

---

2.2.2 4PCS算法

---

PCS算法全称为 4-Points Congruent Sets 即 4点全等集配准算法。该算法也是基于RANSAC算法框架，对两片点云的初始姿态不做约束，针对搜索对应点的策略进行了优化，将基本的三组对应点扩展到了四组具有一定约束性的对应点集，大大增加了算法的鲁棒性，提高了算法的搜索效率，算法的时间复杂度约为。该算法的核心思想就是利用刚体变换中的几何不变性（向量/线段比例、点间欧几里得距离），根据刚性变换后交点所占线段比例不变以及点之间的欧几里得距离不变的特性，在目标点云中尽可能寻找４个近似共面点（近似全等四点集）与之对应，从而利用最小二乘法计算得到变换矩阵，基于RANSAC算法框架迭代选取多组基，根据最大公共点集（LCP）的评价准则进行比较得到最优变换。

---

1. 全等四点集

在4PCS算法中，我们将局部配准点云由三个点扩展为四个点，并且这四个点具有一定的附加约束，如果能够在目标点云中也找到相应的近似满足约束的四点集，我们就将这两个对应四点集称为全等四点集，用于求解点云变换。相比传统的RANSAC配准算法中完全随机采样的方式，通过全等四点集的应用，一方面算法减少了计算量，提高了效率，使得全局搜索更有目标性；另一方面算法使用带有约束的局部四点配准，准确性和鲁棒性更高。四点集的选择和约束标准如下：

• 首先在源点云中随机选择三个点，要求这三点所构成的三角形面积尽量的大（三点确定一个平面，向量叉积可以计算面积），但是三点间的距离不能超过一定的阈值上限，该上限由两片点云的给定重叠率 f 确定。因为三点之间距离越大，配准的鲁棒性越高；但距离过大，三点均在两点云的重叠区域之外了，配准效果又不好。因此需要在满足上限的基础之上，尽可能保证大的三级形面积。若没有给定点云重叠率f，则可以进行f=1.0,0.75,0.5…重叠率递减测试，选择最优变换。
• 确定三点后，源点云四点集中第四点的选择方式为：遍历源点云中所有的点，对每一个点进行计算验证选择最优的第四点。第四点需要与其他三点组成的平面尽可能的共面（即不强制要求四点共面，但第四点到其他三点平面的距离尽可能小），并且第四点与其他三点的距离也要满足距离阈值范围（不能太近不能太远）。
• 源点云中的四点集选择完成后，就可以计算其四点构成的两线段交点的变换不变比，根据不变比在目标点云中遍历搜索对应的满足该约束所有四点集用于配准计算，这就是（近似）全等四点集。

---

2. 4PCS算法流程

在了解了全等四点集这一核心概念之后，我们来介绍一下基于全等四点集寻找对应点的4PCS的算法步骤如下：

• STEP1：在源点云P中，使用上述的四点集的选择方法随机选择一个四点集B={b1,b2,b3,b4}，其中(b1,b2)确定线段1，(b3,b4)确定线段2。接着去计算不变量d1=|b1-b2|，d2=|b3-b4|，不变比r1=|b1-e|/|b1-b2|，r2=|b3-e|/|b3-b4|。注意因为四点不一定共面，两条线段也不一定相交，所以可以使用连接两个线段的最近点的中心点作为“交点”。
• STEP2：在目标点云Q中，遍历所有的点对，筛选满足第一个约束（线段长度在d1或d2误差范围内）的点对集合R1，R2。其表示为：
• STEP3：遍历点对集合R1中的所有点对元素，计算其线段上满足不变比r1的目标交点，然后将所有计算结果e存入搜索树ANN
  Tree（近似最邻近搜索树，最常见的是K-D Tree算法），并构建相应的映射
• STEP4：遍历点对集合R2中的所有点对元素，计算其线段上满足不变比r2的目标交点，并构建相应的映射。然后遍历所有的点，在之前构建的ANN
  Tree中搜索可接受误差范围内的重合点，若可找到则说明能在Q中找到一个对应的近似全等四点集。最终求得所有的近似全等四点集
• STEP5：遍历所有的近似全等四点集，对每一个，通过最小二乘法计算其与B的对应变换矩阵。然后使用该变换矩阵对源点云P进行变换得到P’，统计P’与Q中的最大公共点集（largest common pointset，LCP），记max(LCP)的变换矩阵为本次迭代的最优变换矩阵T并保留。
• STEP6：不断迭代这个过程，记录最优的变换。迭代结束后得到的变换矩阵即为最优变换矩阵。原论文算法框架如下：

---

3. 增强方法

该算法的实现过程中还使用了一些增强方法。比如在上述不变量的基础上，添加了对应点的法向量计算，只有满足线段长度近似相等且端点法向量夹角也近似相等的前提下，才认为是一对对应线段/向量，进一步加强搜索条件，减少了搜索数量。

• Super 4PCS(Super 4-Points Congruent Sets)
• SK-4PCS（Semantic Keypoint 4-Points Congruent Sets）
• G-4PCS（Generalized 4-points congruent sets）

---

2.2.3 SAC-IA算法

算法步骤：
1、分别计算源点云和目标点云的FPFH特征描述子；
2、基于FPFH特征描述子对两个点云中的点进行匹配；
3、从待配准点云P中选取n个采样点，为了尽量保证所采样的点具有不同的FPFH特征，采样点两两之间的距离应满足大于预先给定最小距离阈值d；
4、在目标点云Q中查找与点云P中采样点具有相似FPFH特征的一个或多个点，从这些相似点中随机选取一个点作为点云P在目标点云Q中的一一对应点；
5、计算对应点之间刚体变换矩阵, 然后通过求解对应点变换后的“距离误差和”函数来判断当前配准变换的性能，此处的距离误差和函数多使用Huber罚函数表示；
6、重复3~5步，直到满足条件，将最小误差对应的旋转和位移作为最终结果；

2.2.4 参考链接

点云配准各种方法总结
点云配准（三） 传统点云配准算法概述
点云粗配准算法-4pcs
经典配准算法及配准效果对比
SAC-IA算法实现

2.3 点云精对准

经过粗配准之后，两片点云的重叠部分已经可以大致对齐，但精度还远远达不到我们的要求。精配准算法就是在粗配准的基础上再进行进一步的配准，提升配准的精度。常用的精配准算法有ICP（Iterative Closest Point）算法和NDT（Normal Distribution Transform）算法。

2.3.1 ICP算法

基本思想：
选取两组点云中距离最近的点作为对应点，通过所有对应点对求解旋转和平移变换矩阵，并通过不断迭代的方式使两片点云之间的误差越来越小，直至满足设定的阈值要求或迭代次数。ICP算法要求待配准的两片点云具有较好的初始位置（初始变换R和T），即要求两片点云大致对齐。

算法框架：

ICP算法核心说明：

整个ICP算法迭代可以拆解为两个核心子问题，即寻找匹配最近点和计算最优变换

• 寻找匹配最近点

利用初始变换 、或上一次迭代得到的变换，对初始点云P进行变换，得到一个临时的变换点云P’。然后用这个点云P’和目标点云Q进行匹配，找出源点云中每一个点在目标点云中的最近邻点作为对应点，为接下来的计算最优变换做准备。常见的最邻近点匹配方法有：
暴力循环法：对两点云分别进行双重循环，遍历匹配每一个点对，计算其欧氏距离，选择距离最近的作为该点的对应点并保存。该方法的复杂度为
ANN(Approximate Nearest Neighbor) 搜索：使用近似最近邻搜索算法，将点插入搜索树结构上，最常见的搜索树结构算法是K-D Tree，加速搜索匹配过程，该方法的复杂度为。我们主要使用该KD Tree算法进行查找加速。

• 计算最优变换

在找到最近匹配对应点之后，我们需要计算使得两片对应点云配准的最优变换参数R和T。假设Ps和Pt分别表示源点云和目标点云，则我们的目标优化函数为最小化变换后对应点之间的距离，即 ：

在这里我们将使用SVD奇异值分解来计算最优变换参数，下面给出计算公式：

算法步骤：

• STEP1: 根据粗对准结果初始化变换矩阵Ti，把点云集P转换为P’
• STEP2: 通过最邻近点匹配方法寻找点云集P’和点云集Q的对应点对
• STEP3: 求得使上述对应点对平均距离最小的刚体变换矩阵Ti
• STEP4: 使用新的变换矩阵Ti，把点云集P转换为新的P’，如果新的点云集P’与目标点云集Q满足两个点云集的平均距离小于阈值或迭代次数超出阈值，则停止迭代计算，否则重复2~4步，直到满足迭代条件。

2.3.2 NDT算法

简介：
正态分布变换（NDT）算法是一个配准算法，它应用于三维点的统计模型，使用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快，原文《The Normal Distributions Transform: A New Approach to Laser Scan Matching》。
现实中，LiDAR扫描得到的点云可能和参考点云（如高精地图点云）存在细微的区别，此偏差可能来自于测量误差，也有可能是“场景”发生了一下变化（比如说行人，车辆）。NDT配准可以忽略细微变化，因此可用于解决这些细微的偏差问题。同时，NDT相对与ICP，它耗时稳定，跟初值相关不大，初值误差大时，也能很好得纠正过来。计算正态分布是个一次性的工作，因为其在配准中不利用对应点的特征计算和匹配，不需要消耗大量代价计算最邻近点，所以时间较快。

基本思想：
先根据参考数据（reference scan）来构建多维变量的正态分布，如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好。

算法步骤

2.3.3 参考链接

点云配准算法—ICP（迭代最近邻点）
NDT算法原理与公式推导
点云匹配方法NDT(正态分布变换)

2.4 PCL库点云配准

2.4.1 配准流程

1. 寻找关键点。从两个数据集中按照同样的关键点选取标准，提取关键点。
2. 计算关键点的特征。对选择的所有关键点分别计算其特征描述子。
3. 估计特征的对应关系。结合特征描述子在两个数据集中的坐标的位置，以两者之间特征和位置的相似度为基础，来估算它们的对应关系，初步估计对应点对。
4. 去除不理想的对应关系。假定数据是有噪声的，除去对配准有影响的错误的对应点对。
5. 估计变换矩阵。利用剩余的正确对应关系来估算刚体变换，完成配准，这一步包含粗配准和精配准。
   

2.4.2 参考链接

PCL官方
PCL(Point Cloud Library)学习指南&资料推荐——链接汇总
PCL点云处理算法汇总——相当全面详细，但付费

PCL中点云配准（非常详细，建议收藏）
PCL配准模块总结
PCL点云配准（1）
PCL点云配准（2）
PCL点云配准（3）
SAC-IA粗配准+ICP精配准
SAC-IA粗配准+NDT精配准

2.4.3 PCL安装

Ubuntu18.04安装PCL 1.9.1(图文详解，附踩坑和测试)

3 开源代码

开源3D激光（视觉）SLAM算法汇总（持续更新）
开源3D激光SLAM总结大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及区别

4 LOAM论文学习

前言
LOAM源码可读性非常差，他们实验室后来更新的代码还可以，把代码分成了各个函数；A-LOAM是利用eigen和ceres对LOAM进行了重构，但是没有使用9轴IMU数据消除非匀速运动造成的运动畸变；原始LOAM中关于IMU辅助消除非匀速运动畸变的操作显得十分乱，想理解通还是有一定难度的。

KD-tree和最近邻搜索
LOAM论文精读
LOAM源码加注释

loam中激光里程计部分accumulateRotation()函数， PluginIMURotation函数以及误差函数对欧拉角偏导的数学推导
LOAM 代码部分的公式推导（前端里程计部分）
LOAM_velodyne LaserMapping
A-LOAM(后端1)基于栅格点云地图构建-算法流程+代码注释
激光SLAM-LOAM系列，包含A-LOAM详细讲解、lego-loam、LIO-SAM

5 LeGO-LOAM论文学习

LeGO-LOAM论文翻译（内容精简)
LeGO-LOAM论文阅读（一）–点云分割
LeGO-LOAM论文阅读（二）–特征提取（一）
LeGO-LOAM论文阅读（三）–地图优化
LeGO-LOAM源码加注释

6 LIO-SAM论文和代码学习

LIO-SAM探秘第一章之论文解析
SLAM学习笔记（二十）LIO-SAM流程及代码详解（最全）
LIO-SAM：配置环境、安装测试、适配自己采集数据集

7 激光SLAM杂文

SLAM学习专栏
激光雷达点云数据处理——详细、完整
浅谈：点云库Open3D、PCL以及CGAL（C++）
激光Slam相关算法及其简介

8 因子图优化

3种因子图优化库
SLAM库学习： 从因子图到GTSAM
因子图优化学习—对因子图优化的粗浅理解
因子图优化原理（iSAM、iSAM2论文解析）
因子图优化及GTSAM中IMU预积分接口

9 关于旋转

欧拉角和旋转矩阵之间的转换
欧拉角与旋转矩阵，内旋和外旋的解释
四元数，旋转矩阵，轴角，欧拉角的相互转换（原理+Eigen代码实现）