第五章 基础点云处理

激光传感器与点云的数学模型

1. 雷达分类：机械旋转式雷达和固态雷达
2. 点云测量模型：RAE，距离–方位角–俯仰角
3. 点云的packets表达：内参数放在固定的参数文件，记录运行时变化的部分，可以大幅节省雷达与计算机的数据通讯量
4. BEV:俯视图，定义一个图像中心和分辨率，把点云转换成俯视视角下的图片。当然同理的还有距离图，取图像的横坐标为激光雷达的方位角，纵坐标则取俯仰角，这种投影方式就称为距离图

最近邻问题

准确率和召回率

这个概念其实是一个基础概念，所以就拿到前面说了，首先是假阳性和假阴性，栅格法检测出来的最近邻，实际当中并不是最近邻。这种情况称为假阳性（false positive）。一次实验中假阳性的次数记作FP。而实际当中的某个最近邻，在栅格法中并没有检测到。这种情况称为假阴性（false negative）。一次实验中假阴性的次数记作FN。 precision和recall的计算都是基于这两个概念来的

暴力最近邻

这个其实很简单，就是把所有的点到目标点的距离算一遍，最近邻就找距离最小的，K近邻就找距离最小的K个

栅格和体素方法

栅格我理解就是2D方法，体素就是3D方法，其实就是把点云分成一个个格子，然后选择几个周围的格子来找。然后对格子里的点云，用的其实还是暴力法

二分树与K-d树

根据数据结构的知识我们知道：对一个已经排过序的容器来说，使用二分查找会比线性查找具有更高的效率。而K-d树本质上就是高纬度的二分树，这里的建树流程简单来说就是不停地去找主轴分割，直到所有的点都变成了叶子节点

查找的话涉及到了剪枝的技巧，因为一侧的树到当前点理论上有个最小距离，所以可以根据这个距离判断要不要展开一侧的节点，比如下面这个图右侧的节点就没必要展开了

这里的代码其实更重要，复习的时候还是要结合着代码看，才能理解得更透彻一些
当然，这里还有一个近似最近邻，也就是ANN，其实就是在剪枝得时候增加了一个比例

四叉树和八叉树

这个其实和体素/栅格法以及八叉树都有点像的，不过我理解这里的区别主要就是树的形式更类似一种展开的形式，所以这个格子并不像体素那种就是完全固定的，比如一个地方没有了也就不再展开了

拟合问题

平面拟合

平面拟合问题的本质就是给一定数量的点，然后根据平面约束方程，来拟合平面参数，这个平面方程的形式还是很简单的

上面问题的维度是四维的，而每个点提供的约束相当于是三维的，这样有多个点的时候，由于噪声影响，上述方程大概率是无解的（超定的）。因此，我们往往会求最小二乘解（Linear Least Square），使误差最小化。当然我们可以把坐标进行格式上的变化，类似于齐次坐标这种表达方式，直接表示出来这个最小二乘问题；当然我们进一步地用矩阵来表示所有的点

关于这种最小二乘问题，可以有特征值解法和奇异值解法，特征值的核心思想是最小二乘的最优解就是最小特征值，当然这里因为矩阵本身不一定是方阵，所以求的特征值就是AAT这种形式。
而奇异值分解，就是对A矩阵进行SVD分解，对角线的奇异值也是由大到小排列的，然后把SVD结果代回最小二乘问题，因为SVD得到的U是正交矩阵，所以转换后和特征值法是一样的

所以具体的应用中，只要把所有点列出来，然后直接求解就好了，还是比较简单的

直线拟合

直线拟合的思路和平面拟合是几乎一致的，也是利用点和直线约束方程来拟合直线参数，这里直线方程的形式是

这里的未知数虽然变成了6个，但是点比较多的时候还是一个超定方程问题，所以求解的思路还是最小二乘。不过这里的误差并不是像平面那样直接带入方程等于0，是稍微要计算下距离直线的垂直距离

然后这里就要分析下导数了，因为这个函数不能直接体现出来要求的最小是什么，这里经过转换后可以发现要求的最小可以转换为点云去中心化的一个效果，所以最后的实现也是基于这种方法实现的

最后我们来看一下点云拟合和平面拟合的关系和区别，书上的这个图非常明显，直线拟合的方向应该是一个主方向，而平面拟合方向应该是一个最小方向，这和推理出来的结果是一致的

第六章 2D激光定位与建图

扫描匹配算法

点到点的scan matching

我们把2D激光扫描也看作是观测，把观测模型简单记为

所以这里，z和m可以看作观测量，我们要估计的值是x，根据贝叶斯估计理论，这种问题可以根据最大后验或者最大似然来估计，也就是

点到点的匹配问题，也就是ICP问题，通过迭代计算来寻找最近点，ICP算法将扫描匹配问题分为数据关联和位姿估计两个部分，然后具体的迭代就是交替执行这两个步骤，直到最终结果收敛。首先，我们简单定义二维激光的位姿

那么我们可以把当前机器人观测到的某个点根据这个位姿转换到世界坐标系下

如果我们这个时候已经有这个点对应的最近邻点了，那么我们可以直接通过两个点之间的坐标差值作为误差，对所有的点进行一个整体的最小二乘来求解这个位姿

这里的Jacobian还是很简单的

因为这个问题和形式都很简单，可以通过手写最小二乘来实现，也可以通过优化库来实现，整体效果应该是差不多的

点到线的scan matching

除了ICP，还有一种比较简单的匹配方式就是点到线的匹配，上面查找的是最近邻，这里则是查找K近邻，然后把近邻点一起拟合生成一条直线，再通过点到直线的垂直距离最小这个最小二乘问题，来完成位姿的估计，这种方法也被记为PL-ICP
关于直线的拟合，之前已经介绍过三维的了，二维和三维的相比更简单一些，就是初中大家就学过的斜率和截距就可以比较准确地表示出来了

拟合得到直线后，我们再利用点到直线距离来表示每个点的残差，这里因为分母是和点坐标无关的值，所以最后可以直接表示为分子的形式

这里的倒数并不是最后的导数，因为我们最后要求的是机器人的位姿，所以残差到机器人位姿的雅可比还有加上后边点对位姿的求导（这里的加指的是链式求导），最后得到一个完整的导数形式

实现的话，就和ICP差不多了

似然场法

在二维
SLAM 中，我们通常会把地图按一定分辨率储存为占据栅格地图（occupancy grid）。所以类似于ICP，可以设计一种将扫描数据与栅格地图进行配准的方式，这里提到的方式就是似然场法，这个问题可以从全局的角度来出发，ICP在全局角度下看就是找到了让所有点之间最合适的一种状态，而似然场就是基于这种思想以及场的概念引出的。不过这里的场是有有效范围和分辨率这个概念的，在这本书里，场是随着距离成平方衰减的。测量点在一个场的误差，就可以看作在这个场的读数，也就是距离的平方。
这本书里的场就是基于距离平方值的，似然场围绕每个扫描点产生，随着距离变大而逐渐衰减。 在配准的时候，我们把似然场的读书直接作为配准时的目标函数使用，所以最后求解的最优化问题就是

可以看到这里的求导也是涉及到了链式求导，重点在于前一项场对点的偏导，因为我们已经把似然场表示为图像这种形式了，所以我们可以用图像的思维来表达这个转换关系

这样我们就可以得到一个整体的Jacobian矩阵了

占据栅格地图

这个概念其实是名字表达非常直接的一个，所谓占据栅格，就是以栅格形式（或者以图像的形式）来存储占据概率的地图形式。当然同理还存在着障碍栅格地图，以及语义栅格地图邓。这里有一个前提是，我们关注机器人的重点在于机器人能不能通过这个栅格，所以我们要知道的就是每个栅格有没有障碍物了。 很好理解的做法是，。在扫描地图之前，我们完全不知道地图中是否有障碍物，所以应该将占据概率设0.5。如果我们多次观察到某个栅格中存在物体，那么它的占据概率会逐渐上升至1。反之，如果多次观察到某栅格是可以通行的，就把它的概率下降至0。这里主要介绍了两种方法：

基于Bresenham 算法地图生成

这个方法我觉得其实是很直接得一种，它是一种对直线进行栅格化的算法，通常用于几何直线的矢量化。特点就是完全由整数来实现，所以运算效率非常高。 这里直接给出具体的算法

所以其实这就是找到机器人和末端点之间的连线，认为这些地方是可通行的

基于模板的地图生成

另一种方法则是基于模板的地图生成，模板的意思就是把模板区域内的每个格子的角度和距离都先算好，需要更新栅格地图时，我们将模板当中的每个格子的距离值与激光在该角度下的距离值进行比较。如果模板中的距离小于激光打到的距离，就可以认为该格子是空白的。如果等于激光的距离值，则认为该格子被占据。如果大于激光的距离，则该格子不作更新。这样，我们就可以回避为每个激光线计算栅格化的过程。 这里涉及到了一个具体技巧，为了提升运行效率，这里以127作为初始值，然后观测到障碍物-1，能通过就+1，同时也可以限制一个上下界的范围，这样的话整个运算就都是整数了，而且效率也非常高，这里的具体实现如果想不出来，可以结合代码看一下，代码还是非常清晰的

子地图

把匹配算法和栅格地图放在一起，利用栅格地图的更新机制将匹配好的数据放到一起。进一步，把若干个匹配好的结果放在一起，就形成一个个子地图（submaps）。 这个概念在这本书的2D SLAM部分还是挺重要的，有了子地图我感觉可以直观理解为一段区域内有了地图参考，所以这时候scan to map的过程其实就是扫描到子地图的一个配准了。这样我们可以很轻松地管理世界–子地图–当前扫描之间的坐标关系

接下来就可以根据子地图来实现一个简单的2D SLAM系统，里面比较重要的其实就是关键帧选取以及子地图的更新，因为如果不更新子地图了，那这个子地图相当于又变成全局地图了，这个时候又没有意义了

这里我们需要注意到，每增加一帧的时候，子地图的似然场函数和栅格地图都是要跟着发生改变的

void Submap::AddScanInOccupancyMap(std::shared_ptr<Frame> frame) {occu_map_.AddLidarFrame(frame, OccupancyMap::GridMethod::MODEL_POINTS);  // 更新栅格地图中的格子field_.SetFieldImageFromOccuMap(occu_map_.GetOccupancyGrid());           // 更新场函数图像
}

回环与检测

其实激光SLAM的回环与检测也是很重要的一个部分，因为没有这个部分的话，里程计就是会有累积误差，那最后的定位和建图精度就是会差一些，不过高博这个书里只是简单介绍了下这里的思路。并且对金字塔法和基于子地图的回环修正进行了着重的介绍

金字塔法回环检测

这里其实和视觉的金字塔法有异曲同工之妙，本质上都是通过不同的分辨率，来增大我匹配成功的可能性和准确性。 。它依旧是一个扫描匹配问题，无非是在原先的基础上增加了初始位姿估计不太准确这个条件，我们在小分辨率场下可以容忍更大的误差存在，于是我们把这个结果放到高分辨率的场下，就行成了一个由粗至精的匹配过程，所以这里如果上层匹配失败了，我们自然就不再进行下层的匹配了。

• 这里还涉及到一些工程经验的问题，，我们在多分辨率配准中，只要匹配点数超过所有扫描数据的一定比例就可以认为匹配成功了，但这个时候这个比例就很直接地影响到了回环检测的一个限制尺度，太宽松或太严格的尺度都不是很好，具体的工程问题中应该是要调试来看的

基于子地图的回环修正

这个问题的核心说白了就是下面的公式和一张图

根据当前帧，当前子地图和历史子地图的关系来增加一个约束，下面的残差实现高博是让g2o自动求导实现了

工程细节

1. 运动补偿（3D里是靠IMU进行了一下补偿）和反光（这个确实没想到什么太好的方法）
2. 玻璃场景
3. 特殊运动场景：震动、以及倾斜路段等等

空旷场景和走廊场景。在这两种场景中，2D 扫描匹配算法原则上无法确定激光扫描的位置，其结果可能存在一个或一个以上的额外自由度。我们把这类问题称为退化问题，退化问题在纯激光SLAM 系统中比较明显。如果引入其他传感器，则可以一定程度上补偿退化带来的影响。

第七章 3D激光定位与建图

多线激光的scan matching

ICP

这里其实和2D的一样，ICP是配准的最基本方法之一，其基本原理如下

然后这里的点误差和导数形式也可以表示的很简单

点到线和点到面的ICP

这里和上一章2D中点到线的PL-ICP是类似的，依旧不再是寻找一个最近邻点，而是寻找几个点，然后分别用直线拟合的方法和平面拟合的方法得到一个直线或平面，然后通过点到直线的残差或点到平面的残差来优化整个位姿问题。 先来看一下平面残差的表现形式和导数，平面残差是直接带入平面方程就可以的

然后点到线的话，依旧是算一个垂直距离的问题

这里要注意一下，点线和点面的维度和雅可比都是不太一样的，要记得区分一下

NDT

这个方法在之前autoware的学习中也有复习过，可以说是autoware框架中主要依赖的一个定位方法， 回忆刚才使用到的点线ICP与点面ICP，其实都是把点和一些点的一个统计量进行的配准，因此进一步地，只需要得到一堆点云的局部统计信息，我就能用这些统计信息来进行匹配，而一组点云最近本的信息，就是均值和方差，也就是NDT中所用到的，高博的书中NDT采取了一种相对简单的推导思路

所以这个时候的残差，就是转换后和统计量均值的残差了

这个问题其实就是一个最大似然估计，雅可比的求取也很简单

跟所有配准算法一样，NDT也是十分依赖初值的一个算法，如果初值估计不准，那么很可能出现误匹配的过程。另一方面，NDT 也是依赖体素大小的算法。所有依赖体素的算法都会存在体素的离散化问题，所以对体素的选取也是直接影响最终的一个配准结果的

直接法激光里程计

使用NDT构建激光里程计

这里需要维护一个由多个scan组成的局部地图，然后调用NDT和局部地图来进行一个配准，其中每隔一定距离或一定角度就取一个关键
帧，再把最近的若干个关键帧拼成一个局部地图，作为NDT 算法的配准目标。NDT的初值则由上两帧的一个相对运动模型估计出来

增量NDT

增量NDT的两个直接问题是：如何维护不断增加的体素以及每个体素内的高斯参数应该如何改变，这里关于高斯分布的增量更新，还是可以从最基本的定义来展开，然后通过对公式进行处理来得到

至于体素的增量维护的话，书中是把体素的总量保持在10万个左右，那么很早之前建立的体素就会被定期删除了，所以这里是把最近更新的体素要放在前面，然后队列超出容量限制的时候，把最旧的体素再给删掉，这里实际用到的是把体素数据放到双向链表里，然后hash表来维护迭代器来作为提取的查找索引，同时，每个体素会带有一个标记，标志它内部的高斯参数是否已经被估计。我们为每个体素设计一
个缓冲区，当缓冲区中的点数大于一定数量时，才估计它的高斯参数。

特征法激光里程计

特征提取

高博在书中提到，最初的LOAM和后续各种改进版本，是许多自动驾驶行业常用的开源方案，也是后续很多LIO系统的基础，书中是参考LOAM的原理设计了一个简单的实现。 特征提取法的关键就在于，要提取的是什么特征，什么样的特征对实时配准是有意义的。书中是基于LOAM的原理，针对带有线束信息的激光点云设计了特征提取的思路：通过计算曲率来判断点的类型，特别大的曲率被认为是角点，然后比较小的曲率可能就被认为是平面点了

这里的特征提取算法主要包括三个部分：

所以这里如果点云是不带线束信息的话，可能要先进行一下等效线束的处理，然后才能用这种方法进行特征提取

里程计的实现

这里和NDT的流程有些类似了，也是要构建局部地图，当然这里因为是有特征点类型信息的，所以分别根据角点和平面点来维护两个局部地图，然后再把两个ICP放进同一个优化问题里来一起优化，这里角点是看作点线ICP，然后平面点就根据点面ICP来就好

松耦合LIO实现

之前也有提到，在工程上纯Lidar存在很多问题（2D SLAM中提到），以及初始位姿对Lidar的配准问题的精度还是有直接影响的，因此利用IMU和Lidar进行耦合的系统，理论上应该有比单纯的Lidar更好的表现才对。 在松耦合系统中，IMU和轮速为位姿估计器提供惯性和速度方面的观测源，而雷达点云匹配的结果为这个系统提供位姿数据的观测源，书中这里是基于ESKF实现的。
当然首先因为引入了IMU，所以必不可少的就是IMU与雷达之间的坐标转换关系了，也就是外参，这里比较简单的方式是把雷达观测到的点转到IMU坐标系下，这样IMU的运动模型就不会有额外的参数引入，整个运动模型还是保持在一个比较简单的形式下

和GNSS一样，把雷达的观测也看作为姿态和位置的直接观测，这样旋转和平移也都可以被表达为比较简单的形式，而且对应的雅可比也都是单位阵，便于计算

然后这里高博的书里提到了一个IMU与点云同步，这里需要注意，这个同步不是我们经常说的在硬件层面上的那种同步，而是跟类似于VINS sync函数做的那样，其实就是把两帧之间的IMU数据保存下来了，这样后面在雷达数据到来之前，就可以用对应时间段的IMU数据来进行预测了。 而且这里还有另外一个很重要的作用，就是通过IMU的数据可以很轻松地实现对Lidar的运动补偿，补偿由车辆运动带来的扫描数据畸变。

因为EKF阶段已经可以得到两个扫描之间的相对位姿了，所以我们只要通过插值算法就可以实现最后的运动补偿了

总结

这部分我概括为这本书的第二部分，主要介绍了激光SLAM的各种原理，从很基础的最近邻查找到最后简单的松耦合算法的实现，所以到这里应该可以理解一个LO系统的大概流程以及设计思路，后续的章节的话则会更加偏向实际工程部分了。