CSM算法
算法简介
通过前后两帧激光雷达数据准确的计算出当前机器人的位姿信息
算法步骤
- 估算出观测模型关于位姿x的概率分布，观测模型取最大值时对应的位置认为是对x最准确的估计
- 扩大观测模型概率结果的搜索空间，在搜索空间内计算全部位姿得分，得分最高的即是对x的最准确的估计
- 为了提升效率，原论文中使用了多分辨率查询表加速的策略
EM算法
算法简介
EM算法是一种迭代优化策略，由于它的计算方法中每一次迭代都分两步，其中一个为期望步（E步），另一个为极大步（M步），所以算法被称为EM算法（Expectation Maximization Algorithm）。EM算法受到缺失思想影响，最初是为了解决数据缺失情况下的参数估计问题。
算法步骤
- 首先根据己经给出的观测数据，估计出模型参数的值
- 依据上一步估计出的参数值估计缺失数据的值
- 估计出的缺失数据加上之前己经观测到的数据重新再对参数值进行估计
- 然后反复迭代，直至最后收敛，迭代结束
举个栗子
A、B两枚硬币
Pa：A硬币正面朝上的概率
Pb：B硬币正面朝上的概率
在不知道抛的是哪个硬币的前提下进行多组抛硬币实验
实验观测结果举例：正正正反反。但是不知道是哪枚硬币的正正正反反。
先假设Pa和Pb的值作为初始化（初始化的值设为多少也很重要，设得不好容易陷入局部最优解）
计算如果是硬币A，正正正反反的概率
在计算如果是硬币B，正正正反反的概率
选择概率大的作为假设抛的硬币
将所有组的实验都按照上述步骤进行 并统计所有组硬币A朝上的概率以及硬币B朝上的概率 得出新的Pa和Pb
进行迭代 得出最优的Pa和Pb
ICP算法
算法简介
ICP为求解帧间位姿的算法。
ICP为点云匹配算法中的精匹配。
点云匹配算法分为粗匹配和精匹配
粗匹配为在变换和旋转均未知的情况下进行匹配 目的是为精匹配提供靠谱的初始值
精匹配为根据给定的初始值得到更精确的匹配
ICP也有很多变种 一般区别为loss函数的设定区别 大致分为：点-点 点-线 线-线
算法步骤
- 根据初始值或上一次迭代的结果得到一个临时变换点云
- 在当前点云和临时变换点云上进行对应点的匹配
- 根据匹配的点对的距离构造loss函数
- 对loss函数进行优化 迭代求解
算法加速
在两个点云寻找对应点的环节 全部遍历的话算法过于耗时
论文中采用了两种加速配对的方法：
- 设定阈值 当两个点云的距离小于一定阈值时认为这两个点云为配对点云 即不进行后续的循环查找
- ANN加速查找，常用KD-tree
粒子滤波算法
算法简介
适用于非线性非高斯分布的状态估计 在贝叶斯滤波的基础上对先验概率和后验概率进行了修改 即不给出先验和后验的函数
用一堆粒子去进行状态估计 迭代一次后的粒子将向更靠近正确的状态估计的方向移动。
重采样算法 那些权重较大的粒子将更多的被选出
NDT算法
算法简介
正态分布变换（NDT）算法帧间位姿变化求解算法，使用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快。
NDT算法的基本思想是先根据参考数据（reference scan）来构建多维变量的正态分布， 如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。
算法步骤
- 将参考点云所占的空间划分成指定大小的网格或体素，并计算每个网格的多维正态分布参数
- 初始化变换参数（赋予零值或者使用里程计数据赋值）
- 对于要配准的点云，通过变换T将其转换到参考点云的网格中
- 根据正态分布参数计算每个转换点的概率密度
- NDT配准得分通过对每个网格计算出的概率密度相加得到
- 根据牛顿优化算法对目标函数（得分函数）进行优化，即寻找变换参数p使得score的值最大。（迭代求解）
特征点匹配算法
scan to scan
将所有特征点转移到当前帧的坐标系下，再将前后帧的点云转移到同一坐标系下 进行点云匹配
根据线特征和面特征的点构建不同的loss函数（点到直线或平面的距离）获得粗位姿并根据粗位姿进行建图 将粗位姿交给scan to map算法进行优化 得到精位姿
这一步也可以交给IMU或者视觉里程计去做
scan to map
和scan to scan的区别：
- scan to map将使用更多的特征点
- 线特征和面特征获得相邻点的方式不同，在scan-to-scan中是通过直接搜索最近的激光点获得的直线上的两个点或者平面上的三个点，但是在scan-to-map中是搜索最近的5个点，然后通过主成分分析或者最小二乘的方式获得直线和平面的表达式，最后通过采样来获得最后的匹配点，这样做显然会更加鲁棒。
slam的精度取决于scan to map的精度
