Reference:

1. 高翔，张涛 《视觉SLAM十四讲》
   
2. 激光雷达自动驾驶障碍物检测理论与实践
   

激光雷达使用激光束感知三维世界，并通过测量激光返回所需的时间来输出点云。它集成在自动驾驶、无人机、机器人、卫星、火箭等多个领域。

1. 介绍

1.1 激光雷达-一种三维激光传感器

激光雷达传感器利用光原理进行工作，激光雷达代表光探测和测距。它们可以探测到 300m 以内的障碍物，并准确估计它们的位置。在自动驾驶汽车中，这是用于位置估计的最精确的传感器。
激光雷达传感器由两部分组成：激光发射（顶部）和激光接收（底部）。发射系统的工作原理是利用多层激光束，层数越多，激光雷达就越精确。层数越多，传感器就越大。激光被发射到障碍物并反射，当这些激光击中障碍物时，它们会产生一组点云，传感器与飞行时间（TOF）进行工作，从本质上说，它测量的是每束激光反射回来所需的时间。如下图：
当激光雷达的质量和价格非常高时，激光雷达是可以创建丰富的三维环境，并且每秒最多可以发射200万个点。点云表示三维世界激光雷达传感器获得每个撞击点的精确位置。
激光雷达传感器可以是固态的，也可以是旋转的，固态激光雷达将把检测的重点放在一个位置上，并提供一个覆盖范围（比如FOV为90°）。在后一种情况下，它将围绕自身旋转，并提供360°旋转。在这种情况下，一般把它放在设备顶上，以提高能见度。

激光雷达很少用作独立传感器。它们通常在称为传感器融合的过程中与相机或雷达结合使用。融合过程可分为早期融合和晚期融合。早期融合是指点云和图像像素的融合，晚期融合是指单个检测对象的融合。

1.2 激光雷达的优缺点？

缺点：

• 激光雷达无法直接估计速度。他们需要计算两次连续测量之间的差异。
  
• 激光雷达在恶劣的天气条件下无法正常工作。在雾或雨中，激光会击中它，使场景变得混乱。
  
• 激光雷达的价格虽然下跌，但仍然很高。
  

优势：

• 激光雷达可以准确估计障碍物的位置。到目前为止，还没有更准确的方法。
  
• 激光雷达处理点云。如果我们看到车辆前方的点云，即使障碍物检测系统没有检测到任何东西，我们也可以及时停车。这是一个很大的安全保证，车辆将不仅依赖于神经网络和图像的概率问题。
  

1.3 基于激光雷达如何进行障碍物检测？

激光雷达进行障碍物的步骤通常分为 4 个步骤：

1. 点云处理
   
2. 点云分割
   
3. 障碍聚类
   
4. 边界框拟合
   

1.4 点云处理难点

1. 疏;
   
2. 不规则 – 难以寻找邻居；
   
3. 缺乏纹理信息；
   
4. 无序——深度学习很困难；
   
   如果上面的行改变了，表达式仍然是同一个对象。对于深度学习，不同的输入矩阵会产生不同的输出，但我们希望输出相同。
   
5. 旋转同变性/不变性：我们旋转一些点，它的坐标是不一样的，但是它还是同一个物体。
   

2. 点云处理

2.1 点云处理-体素网格

为了处理点云，我们可以使用最流行的库 PCL(point cloud library)。它在 Python 中可用，但是在 C++ 中使用它更为合理，因为语言更适合机器人学。它也符合 ROS(机器人操作系统)。PCL 库可以完成探测障碍物所需的大部分计算，从加载点到执行算法。这个库相当于 OpenCV 的计算机视觉。因为激光雷达的输出很容易达到每秒 100000 个点，所以我们需要使用一种称为体素网格的方法来对点云进行下采样。

2.1.1 什么是体素网格？

体素网格是一个 3D 立方体，它通过每个立方体只留下一个点来过滤点云。立方体越大，点云的最终分辨率越低。最终，我们可以将点云的采样从数万个点减少到数千个点。
滤波完成后我们可以进行的第二个操作是 ROI(感兴趣区域) 的提取，我们只需删除不属于特定区域的每一些点云数据，例如左右距离 10m 以上的点云，前后超过 100m 的点云都通过滤波器滤除。现在我们有了降采样并滤波后的点云了，此时可以继续进行点云的分割、聚类和边界框实现。

3 三维点云的分割

3.1 RANSAC

点云分割任务是将场景与其中的障碍物分离开来，其实就是地面的分割。一种非常流行的分割方法称为 RANSAC(Random Sample consenses)。该算法的目标是识别一组点中的异常值。点云的输出通常表示一些形状。有些形状表示障碍物，有些只是表示地面上的反射。RANSAC 的目标是识别这些点，并通过拟合平面或直线(拟合的是地面)将它们与其他点分开。
为了拟合直线，我们可以考虑线性回归。但是有这么多的异常值，线性回归会试图平均结果，而得出错误的拟合结果，与线性回归相反，这里的 RANSAC 算法将识别这些异常值，且不会拟合它们。

如上图所示，我们可以将这条线视为场景的目标路径（即道路），将异常值视为障碍物。

3.1.1 RANSAC 的实现

过程如下：

• 随机选取2个点
  
• 将线性模型拟合到这些点会计算到每隔一个点的拟合线的距离。如果距离在定义的阈值距离容差范围内，则将该点添加到内联列表中。
  

因此，需要算法的一个参数：距离阈值。

最后选择内点最多的迭代作为模型；其余的都是离群值。这样，我们就可以把每一个内点视为道路的一部分，把每一个外点视为障碍的一部分。RANSAC应用在3D点云中。在这种情况下，3个点之间的构成的平面是算法的基础。然后计算点到平面的距离。

下面点云为 RANSAC 算法的结果，紫色区域代表车辆（RANSAC 在这里应该只用来区分地面了）：

RANSAC 是一个非常强大和简单的点云分割算法。它试图找到属于同一形状的点云和不属于同一形状的点云，然后将其分开。

4. 障碍聚类

4.1 点云聚类

RANSAC 的输出是障碍点云和地面。由此，可以为每个障碍定义独立的簇。它是如何工作的？

聚类是一系列机器学习算法，包括：k-means（最流行）、DBScan、HDBScan 等。这里可以简单地使用欧几里德聚类，计算点之间的欧几里德距离。

4.1.1 计算 KD-Tree

在进行点云聚类问题时，由于一个激光雷达传感器可以输出几万个点云，这将意味有上万次的欧几里德距离计算。为了避免计算每个点的距离，这里使用 KD-Tree 进行加速。

KD-Tree 是一种搜索算法，它将根据点在树中的XY位置对点进行排序，一般的想法-如果一个点不在定义的距离阈值内，那么x或y更大的点肯定不会在这个距离内。这样，我们就不必计算每一个点云。

4.1.2 欧式聚类

过程如下：

• 选取两个点，一个目标点和一个当前点；
  
• 如果目标与当前点之间的距离在距离容差范围内，则将当前点添加到聚类中。
  
• 如果没有，请选择另一个当前点并重复。
  

点云欧几里得聚类算法就是将一组点云按照距离进行划分。聚类算法将距离阈值、最小聚类数和最大聚类数作为输入。这样就可以过滤掉“幽灵障碍物”（空间中没有理由存在的单点云），定义一个封闭的障碍物距离。如下图所示，使用不同的颜色来表示聚集的障碍点云簇。

4.2 最邻近(NN)问题

• K-NN
  给定空间中的一组点，一个查询点，在中找到最近的点。
  
  
• Fixed Radius-NN
  给定空间 中的一组点 ，一个查询点 ，找出 中与 匹配的所有点。
  
  

4.2.1 为什么 NN 问题很重要

1. 它几乎无处不在
   
2. 表面法线估计
   
3. 噪声过滤
   
4. 采样
   
5. 聚类
   
6. 深度学习
   
7. 特征检测/描述
   
8. 为什么不简单的直接调用开源库（flann, PCL, ect.）
   
9. 它们不够高效：它们是通常使用的库，没有对 2D/3D 做优化；大多数开源的八叉树实现是低效的，而八叉树在 3D 中是最有效率的
   
10. 很少有 NN 库基于 GPU 的
    

4.2.2 为什么点云的 NN 很困难

1. 不规则：点云可以分布在任何地方
   
2. 维度的诅咒：点云可以是三维的，与二维相比，数据量是指数级的。也可以搭建一个3D的网格，将点云转换成类似图像的东西，但是大部分的网格都是空白的，网格大小的选择也是一个问题。所以使用网格效率不高。
   

4.2.1 为什么最邻近在点云中如此困难

图片：
邻居简单地用表示。
点云：

• 不规则：可以分布在图像的任何地方；
  
• 维度灾难：网格大多是空白的，原则上是低效的。
  

4.3 BST(Binary Search Tree), kd-tree, octree 核心思想

• 空间划分：
  1. 将空间划分成不同面积；
  2. 只寻找一部分区域，而不是所有的数据点。
  
• 停止条件：
  1. 如何跳过一些区域：每个区域都会有一个最差距离；
  2. 如何停止 k-NN/radius-NN 查找: 如果知道可能的结果都在某个区间里面，那么查找完就结束了；
  

4.4 二叉树

BST 是一个基于节点的树结构：

1. 左边的 key 都要比中间 root 小，右边的大。
   

一个节点包含：

• Key；
  
• Left child;
  
• Right child;
  
• …
  

4.4.1 BST 构建/插入

左边小，右边大。如果左右两边都被占用了，继续对比左右两边被占用。
给定一个一维点集，比如一个数组：

4.4.1.1 BST 插入复杂度

最坏的情况是，在这里为在 BST 中点的个数。如将数组 [9,8,7,6,5,4,3] 按顺序插入进一个空的 BST 中：

这是一棵合法的二叉树，但它没用。平衡二叉树是另一个主题。最佳情况：。

4.4.2 BST 查找

给定一个 BST 和一个待查找 key，决定哪一个 node 等于这个 key，如果没有，则返回 NULL。

4.4.3 1NN 查找最邻近

比如说在下图中查找点 11：
1. 根节点为 8，这时最坏距离为 11-8=3。11比8大，这时知道要查找的范围在（8,14）之间，右边子树在范围（8，+inf），往右边看；
2. 到节点 10，这时最坏距离为 11-10=1，这时知道要查找的范围在（10,12）之间，因此不用往右边看，这时右边是比 10 大的，往右边看；
3. 到节点 14，最坏距离依旧为 1。这时要不要找 14 左边子节点？是需要的，左边的子节点比 14 小，要找的仍是（10,12）之间；
4. 找到 13，发现没什么变化，这时子节点已经遍历完了，退回 14；
5. 同理退回 10；
6. 同理退回 8

这节省了五次比较操作。

4.4.4 kNN 查找最邻近（N 个最邻近）

• 做法上几乎与 1NN 完全相同，区别在于计算最坏距离。
  

原始二叉树更难用于高维最近邻搜索，因为二叉树搜索方法依赖一个属性：左侧小，右侧大。对于一维数据，您可以比较距离，但如果这是高维数据，则不能。

4.5 Kd-tree

Kd-tree 就是在每个维度上做一个二叉树。但是它有一个更为复杂的地方是，每一个节点包含很多内容。

4.5.1 Kd-tree的构建

• 如果只有一个点，或者 点数
• 否则，选择一个垂直于所选分割轴的超平面（三维是一个平面）并将该点分成两半；
  
• 反复重复前两个步骤。
  

4.5.2 Kd-tree的两种方式

左边的案例是找一个超平面，把超平面放在数据点上；
右边的超平面不属于任何点。

以下是砍树的两种方法。这里的波浪线是两位数的数据。第一步是在中间切刀，第二步：

• 1.之前竖着切了一刀，后面切就是水平的了，轮流切；
  
• 2.有另外一种切法，叫做自适应。因为我们切这一刀是为了让点在每个维度的分布上更加平均。图中的数据即使中间切了一刀，其他数据也更像是水平分布的，因此再在水平来两刀。
  

两种方式的结果都是一样的，只是第二种方式可能更快一些，并使树更平衡。

5. 边界框

最终的目标是围绕每个点云簇创建一个三维边界框。因为我们没有对点云簇进行任何分类，所以我们必须将边界框与点云相匹配。主成分分析(PCA)是一种有助于拟合边界框的算法。

5.1 PCA

PCA 用来找到点云的主方向。物理意义：将数据点都投影到一个非常有特征性的方向上 ，每一个点在这个方向上的投影就是主成分。如下图所示，在三维情况下，这个椭圆的主成分就是它的三个轴。

5.1.1 PCA 应用

• 降维；
  
• 平面法线估计；
  
• Canonical orientation(典型方向?)；
  
• 关键点检测；
  
• 功能描述。
  

5.1.2 Singular Value Decomposition (SVD)

比如有一个矩阵，可以分解为、、，其中和是正交矩阵； 是对角矩阵，在对角线上形成的特征值。

假设我们将乘以一个向量：

1. 的应用其实是向量在高维空间的一次旋转，所以圆是旋转的；
   
2. 将旋转后的向量在各个维度上进行缩放，使圆变成椭圆；
   
3. 然后套用，也是高维的旋转矩阵，所以椭圆再次旋转。
   所以让 M 乘上一个向量，就是把一个圆变成了一个椭圆。
   

5.1.3 PCA 理论

• 输入：，其中是高维空间中的向量。
  
• 输出：
  主向量，其中为主向量，描述了点组中最具代表性的高维方向。
  

1. 什么叫做最主要的成分？

如果这堆高维点向某个方向投影，这些投影点的方差最大。也就是说，点在这个方向上非常分散。

2. 如何得到第二主要的成分？

去掉这组输入中所有属于的组件，然后在剩下的东西中找出最重要的组件。

3. 如何得到第三主要的成分？

同上，去掉第一种和第二种主要成分。

总结：
输入：，怎么做 PCA？

1. 规范化数据，即减去数据的中心：
   
   
2. 计算 SVD：
   
3. 主向量是的列，第一个主向量是的第一列，第二个是第二列，以此类推。
   

5.1.4 PCA 应用一：降维

将高维数据点投影到低维，尽可能保留其特征。

给定，使用 PCA 找到它的个主向量:

1. 从维度压缩（映射）到维度。
   Encoder
   ：计算每个在每个方向上的投影：
   
   
2. 如何从维度回到维度：
   Decoder
   ：
   
   

降维再升维后，数据肯定会有损失，但是损失比较小。

从图中可以看出，投影到主向量上，依然可以看到有两个肿块；第二个主向量上只有一个峰值。还可以再次声明，在主向量上保留了更多信息。

5.1.5 Kernel PCA

在 5.1.4 中提到的都是普通的 PCA，普通 PCA 是线性的，因为矩阵乘法其实也就是一个线性操作（矩阵乘上一个向量其实是对一个矩阵的线性组合）。在遇到的数据不是一个线性的情况下，该怎么办？如左图的同心圆，做线性PCA很难区分开来，如右图所示，会得到两条线，没给出特别有意义的信息，比如将他们区分开。这时可以考虑在更高维度下进行操作。

• 原始数据：
  
• 改进数据：，这里选择的第三维的平方和就是极坐标的意思。
  由上面右图可以看出，可以找到一个平面将它们区分开。这时我们只需要做一个普通的线性 PCA，就可以将它们区分开了。只不过这个时候是三维空间，不是二维空间。
  

刚刚所做的操作就是 kernel PCA。

Kernel PCA 步骤：

1. 输入数据，非线性映射
   
2. 与标准线性 PCA 在升维空间上一样：
   2.1 假设均值为零：
   2.2 计算协方差矩阵，加个波浪线因为是在高维空间上操作的，与原来的区分开
   2.3 解特征值/特征向量
   

仍然存在的问题：

• 如何定义映射？
  
• 维度增加后的维度可能非常高。有没有办法避免高维运算，节省计算资源？
  

特征向量可以表示为数据点的线性组合—–求系数，求特征向量。
证明：
其中是一个标量，所以最终的特征向量是的线性组合。

常用核函数的选择：

• Linear
  
• Polynomial
  
• Gaussian
  
• Laplacian
  

一般来说，只能通过实验来判断哪种效果更好，除非你对数据有很好的理解，知道使用什么样的核函数。

真步骤：

1. 选择一个核函数，组合成一个 Gram matrix
   
2. 标准化，使得高维空间的平均值为0，其中为经过处理的核函数矩阵；是数值全为的常数矩阵，即：
   
   
3. 解的特征值/特征向量：
   
   
4. 归一化 使其为模，即
   
5. 取任意数据点并将它们投影到主向量上：
   
   

例子：
如下图所示，输入数据在线性 PCA 下不可分：

当 kernel PCA 使用二次多项式核函数时，如左图所示，点通过第一映射就很明显的区分开了：

也可以使用高斯核函数，如上右图，区分度也很好。

5.1.5 PCA 应用

1. 计算3D 点云的平面法向量
   1.1 选取一个点；
   1.2 找出这个点的邻域；
   1.3 对邻域里的点做 PCA；
   1.4 在邻域里选取的法向量是最不显著的向量；
   1.5 曲率->特征值的比例。
   

/*************************** 额外的******************** * ************/

6. 滤波

• 去噪
  1.1 离群值清除(Raduius Outlier Removal)
  1.2 统计离群值清除(Statistical Outlier Removal)
  
• 下采样：在减少计算量的同时保存特征
  2.1 体素栅格降采样(Voxel Grid Downsampling)
  2.2 最远点采样(Farthest Point Sampling)
  2.3 法向量空间采样(Normal Space Sampling)
  
• 上采样/平滑
  3.1 双边滤波(Bilateral Filter)
  

6.1 Noise removal

6.1.1 Radius Outlier

1. 对于每个点，在半径处找到它的邻居；
   
2. 如果邻居数为，则删除该点。
   

6.1.2 升级版：Statistical Outlier Removal

1. 对于每个点，在半径处找到它的邻居；
   
2. 计算每个邻居离这个点有多远，是这个点，是邻居；
   
3. 使用高斯分布建模距离—–这里是对所有点的整体建模结果，不是针对单个点
   
   
4. 对于每个点，重新计算到邻居的平均距离（半径仍然是）；
   
5. 如果平均距离大于某个阈值，则删除此点，例如在以下情况下删除点时：
   下图为 Statistical Outlier Removal 的处理结果，右边红色为处理前的平均距离，经过滤波后邻居就特别近了。
   
   

6.2 Downsampling

6.2.1 Voxel Grid Downsampling

1. 构建包含点云的体素网格；
   
2. 在每个单元格中选择一个点。
   

这里有两个问题：

1. 这个点怎么选？
   
2. 这个算法如何变得高效？