– 第五章 激光前端配准算法（基于优化/势场方法）
以下为基于势场的方法，与ICP算法不一样，以下算法都不需要匹配点；简单来说就是对某段连续的激光点（障碍物）进行高斯平滑/膨胀，再用分数评价，离激光点（障碍物）越近，得分越高（障碍物本身得分为零），人为地构造出一个得分势场； 这样只要比较得分即可，不需要匹配点。
- 基本数学概念：
a.梯度gradient，由多元函数的各个偏导数组成的向量
以二元函数为例，其梯度为：
b.黑森矩阵Hessian matrix，由多元函数的二阶偏导数组成的方阵，描述函数的局部曲率，以二元函数为例。可用迭代法求解，地址
c.雅可比矩阵 Jacobian matrix，是多元函数一阶偏导数以一定方式排列成的矩阵，体现了一个可微方程与给出点的最优线性逼近。以二元函数为例，
如果扩展多维的话F: Rn-> Rm，则雅可比矩阵是一个m行n列的矩阵：
雅可比矩阵作用：如果P是Rn中的一点，F在P点可微分，那么在这一点的导数由JF§给出，在此情况下，由F§描述的线性算子即接近点P的F的最优线性逼近：
d.残差 residual，表示实际观测值与估计值(拟合值)之间的差值
e. 协方差矩阵
资料：
http://www.360doc.com/content/16/0121/13/13800296_529534763.shtml
协方差矩阵始终是一个对称矩阵，其对角线是方差，非对角线是协方差；
5.1高斯牛顿优化方法
例如：hector-slam。
算法解析：https://blog.csdn.net/qq_39521554/article/details/79919041
** 高斯牛顿法解决非线性最小二乘问题的最基本方法，并且它只能处理****二次函数**
– 5.1.1 原理图示及数学描述
描述：此图表示红色最高点为选取的初始值，取不同的初始解，会得到不一样的结果。（非凸优化、初始值敏感）。下图就是人工势场。
给定目标函数E(T):
- 上图公式：
1、目标是求T（机器人位姿）的极小值；
2、[1-M(Si(T))]²，表示势场（其中Si表示得分，M(x)表示得到坐标X的地图占用概率），即表示在障碍物周围一定区域内，得分表示为1，超过区域表示为0，这就有了一个人工势场（ [0, 1]闭区间 ）。
3、生成T的坐标矩阵，将T全部转换到三维空间中。
– 5.1.2 求解方法步骤
- pi表示当前激光帧的第i个激光点参考坐标/势场坐标；
- M(Si(T))为非线性函数；一阶泰勒展开得线性函数，化简
- M(Si(T))是离散的，无法求导，故需要插值。
- 地图的插值方法：
1、地图双线性插值
如上图，Pm为激光点，已知Pij四点栅格值，且知道其各自的势场值，就可用双线性插值方法推出Pm的势场值；
- 拉格朗日插值法
一般来说多项式插值就是求n-1个线性方程的解，拉格朗日插值即是基于此思想。拉格朗日创造性的避开的方程组求解的复杂性，引入“基函数”这一概念，使得快速手工求解成为可能。
定义：求作<=n 次多项式 pn（x），使满足条件pn（xi）= yi，i = 0，1，…，n.这就是所谓拉格朗日（ Lagrange）插值；
5.2 NDT方法
主要用在3D-slam算法当中。
– 5.2.1 基本思想
NDT算法的基本思想是先根据参考数据（reference scan）来构建多维变量的正态分布，如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。因此，可以考虑用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好。
Normal Distance Transform 正态分布变换；
1、把空间用cel进行区分
2、用高斯分布区代替势场，膨胀为符合正态分布的概率密度区域，形成一个天然分段连续的势场：
把落入cell的点都认为符合拟合的高斯分布。
– 5.2.2 数学描述
– 5.2.3 算法流程
NDT算法的C++实现：
https://blog.csdn.net/adamshan/article/details/79230612
5.3 相关匹配方法及分支定界加速
CSM、BB
例如：cartographer。
CSM作用：先用相关性扫描匹配（CSM）给一个初值，然后构造一个最小二乘问题；用激光末端点匹配取到占据栅格地图中的值，获得得分，取得分最高的作为初值。加上多分辨率计算可以加速，并且获得分辨率意义下的最优解。
相关性扫描匹配的思路其实非常简单。想象给定一个栅格地图，再给定当前帧的点云，怎样才能知道激光雷达所在的位置呢？最简单的办法，**把激光雷达放在地图的每个格子上，看在这个位置时，**点云是否与地图重合，重合程度最高的位置就是激光雷达的真实位姿。
- 基本概念
枚举似然场中的每一个位姿，选择最好的。
– 5.3.1 算法流程
1、构造似然场，即对其进行高斯模糊；
2、指定一个小车的位姿范围，例如1m x 1m，30度范围内，分成许多位姿，计算每一个位姿的得分，取最优解。
3、位姿搜索
- 位姿搜索
1、暴力搜索
三层嵌套for循环，大量三角函数计算。
2、预先投影搜索
把1的顺序调转。
3、多分辨率搜索
因为25cm和2.5cm的差距，保证不了最优位姿在栅格内，所以有时候会得到一个次优解。分支定界可能最优。
5.4 分支定界算法
– 5.4.1 基本思想
– 5.4.2 分支定界的加速作用
