SLAM算法 - 3D激光匹配算法

1 基本方法

1.1 点云表示

点云数据中包含了每个点的经纬度坐标、强度、多次回波、颜色等信息。3D激光点云数据里所包含的信息：
（1）x.y.z坐标信息
根据采集的x.y.z坐标，可直接获取被测物体的三维结构信息，三维结构信息是其他地理信息的载体。
（2）回波次数
回波数是某个给定脉冲的回波总数。从激光雷达系统发射的激光脉冲会从地表面和地表上的物体反射：汽车、行人、以及桥梁等等。
发射出的一个激光脉冲可能会以一个或多个回波的形式返回到激光雷达传感器。任何发射出的激光脉冲在向地面传播时，如果遇到多个反射表面则会被分割成与反射表面一样多的回波。
(3) 强度信息
强度是反映生成某点的激光雷达脉冲回波强度的一种测量指标(针对每个点而采集)。不同物体对于激光雷达的反射程度不同，所以通过强度信号可以对物体进行区分。
(4) 类别
每个经过处理的激光雷达点可拥有定义反射激光雷达脉冲对象类型的分类。可将激光雷达点分成多个类别，包括汽车、路面等。
(5) RGB
可以将RGB(红、绿、蓝)波段作为激光雷达数据的属性。此属性通常来自激光雷达测量时采集的影像。
(6) 扫描方向
扫描方向是激光脉冲向外发射时激光扫描镜的行进方向。

用数学的方式描述：
(1) 空间中的一个点：

（2）所有的点构成点云：

点云质心:
点云的重心是一个点坐标，计算出云中所有点的平均值, 可以说它是“质量中心”，认为是物体质量集中于此点的假想点。
物体质心公式：

在点云中，各点质量为1:

1.2 ICP方法

1.2.1 问题建模

问题模型：求解两帧点云(P和Q)之间的变换关系，找到最佳的旋转R和平移t, 使得经过变换后两帧点云之间的距离误差最小。
假设3D传感器在不同位置下采集两帧点云：

转化为最小二乘优化问题：

在误差函数中加入质心：

1.2.2 算法流程

计算流程如下：

1.2.3 旋转计算(SVD)

按照上面过程，只需要重点解决R的计算，就可以解决平移，接下对R的优化函数进行展开简化：

关于SVD分解的原理可以看《矩阵论》里面的矩阵分解章节，不进行具体介绍。

1.3 NDT方法

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

1.3.1 预处理

划分网格，计算统计特性

1.3.2 问题建模

构造最小误差问题模型

从形式来看，NDT形式是多维正态分布的极大似然函数，NDT匹配计算的本质就是一个多维正态分布的极大似然估计。

1.3.3 优化求解

优化方法在实现上采用了牛顿法，但是为了增加鲁棒性，只采用了牛顿法计算出来的迭代方向作为步长搜索的初始方向。
优化问题属于非线性优化的基本问题，可以参照相关大学或研究生优化课程，不详细介绍。非线性优化方法小结

改进NDT
在点云预处理阶段，采用不同的采用策略，比如随机采用，均匀采样，法空间采样等，提高效率。
在栅格策阶段，采用从固定尺寸栅格到基于八叉树的变化尺寸策略。

参考资料

[1] https://www.cnblogs.com/lvchaoshun/p/7823306.html
[2] https://zhuanlan.zhihu.com/p/56598915
[3] https://www.cnblogs.com/YongQiVisionIMAX/p/13871721.html