3D激光雷达SLAM算法学习03——前端里程计方案

1.本篇大纲

2.前端里程计综述

解决的问题：由原始点云信息求解得到两个关键帧之间的相对位姿约束
输入：原始点云信息
输出：两个时刻之间的位姿关系 P = [R T] （4x4）
主流流程: 点云预处理（提取特征）→建立Loss模型→最优化模型求解→得到P矩阵

3.直接匹配的ICP系列

**1）思路：**对原始点云进行采样→寻找最近关联点建立Loss模型→求解R，T
2）主要方法框图：

3）模型推导
下面以其中点到点+SVD分解求解为例，进行模型建立和推导
loss模型：
$$①E(R,T)=\frac{1}{N}\sum _{i=1}^N||x_i-R{y}_i-T|{|}^2$$
$$X=x_1,x_2,...,x_N$$$$Y=y_1,y_2,...,y_N$$
其中N表示点云预处理后点云个数，X,Y为前后两个关键帧的点云，其中假设x1和y1为最邻近点
Loss模型的思路：是找一个位姿变换关系（用R，T体现）使两个点云中各对对应点尽可能重合，也就是让E（R，T）最小。
求解推导：
$$②E(R,T)=\frac{1}{N}\sum _{i=1}^N||x_i-R{y}_i-T-u_x+R{u}_y+u_x-R_y|{|}^2$$
$$③E(R,T)=\frac{1}{N}\sum _{i=1}^N(||x_i-u_x-R(y_i-u_y)|{|}^2+||u_x-R{u}_y-T|{|}^2)$$

由①到②是一步构造，目的就是为了转化成③，展开说说：
其中$u_x$表示X点云的均值，直观理解就是中心，最后转化成③的形式可以看出来，Loss模型分为前后两部分，前部分只与R有关，后部分当已知R总可以找到一个T使该部分为0，所以整体思路变成先求解前部分求出R，然后用已知R令后部分为0求出T。
③中令$x_i^{{}^{\prime }}=x_i-u_x$ 表示“去除”自身中心后的点云，这个$x_1^{{}^{\prime }}$可以求出来
可以得到：$$④E_1(R,T)=\frac{1}{N}\sum _{i=1}^N||x_i^{{}^{\prime }}-R(y_i^{{}^{\prime }})|{|}^2$$
至此这个问题被完全转化为数学问题，剩下的公式推导基本就是SVD分解的部分，只写出公式不做讲解
$$\begin{gathered} min(E_1(R,T))=min(\frac{1}{N}\sum _{i=1}^N||x_i^{{}^{\prime }}-R(y_i^{{}^{\prime }})|{|}^2) \\ =min(\frac{1}{N}\sum _{i=1}^N(x_i^{{}^{\prime }T}{x}_i^{{}^{\prime }}-y_i^{{}^{\prime }T}{R}^{T}R{y}_i^{{}^{\prime }}-2x_i^{T^{\prime }}R{y}_i^{{}^{\prime }})) \\ =max(\frac{1}{N}\sum _{i=1}^N(2x_i^{T^{\prime }}R{y}_i^{{}^{\prime }})) \\ =max(\sum _{i=1}^N(x_i^{T^{\prime }}R{y}_i^{{}^{\prime }})) \\ =max(\sum _{i=1}^{N}Trace(x_i^{T^{\prime }}R{y}_i^{{}^{\prime }})) \\ =max(\sum _{i=1}^{N}Trace(R{y}_i^{{}^{\prime }}{x}_i^{T^{\prime }})) \\ =max(Trace(RH)) \\ H=\sum _{i=1}^N{y}_i^{{}^{\prime }}{x}_i \end{gathered}$$
至此问题转化为找到一个R使得Trace(RH)最大，那其实就是矩阵分析的内容，就是让RH可以分解成$A^{T}A$的形式，这部分就不写了
最后 $T=u_x-R{u}_y$

4.直接匹配的NDT系列

1）思路：点云预处理→空间划分栅格，计算每个栅格内点云的均值+协方差→由联合概率构建Loss函数→求解R，T
思路讲解：NDT的整体思路也是，找到一个位姿关系R，T，使得Y点云在R旋转T平移后和X点云进可能的重合，与ICP不同的是描述重合的Loss函数，NDT用点云分布的概率密度来描述，将联合概率作为Loss函数，举一个简单的一维例子：
10个点云，由四个分布在＜0，六个分布＞0，如果你求解的RT是对的，那么对应点可以对应上，那么联合概率相当于0.40.4+0.60.6 = 0.52
如果你求解RT正好“完全不对”，那么联合概率相当于0.40.6+0.60.4 = 0.48<0.52
(这个一个简化理解的案例，不代表公式推导)
2）主要方法框图：

3）模型推导
loss模型
$X=x_1,x_2,...,x_N$
$Y=y_1,y_2,...,y_N$
NDT的推导部分主要是方便理解思路，忽略数学概率部分的推导。
划分栅格的目的是将点云分布用正态分布的形式进行描述，假设将点云X分为一个3X3X3的栅格（像一个最简单的魔方），那么假设第一个栅格内点云个数为$nu{m}_1$,注意${\sum }_{j=1}^{27}(nu{m}_i)=N$,在第一个栅格内，进行均值和协方差求解
$$u_1=\frac{1}{nu{m}_i}\sum _{i=1}^{nu{m}_1}{x}_i$$
$${\sum }_1=\frac{1}{nu{m}_1-1}\sum _{i=1}^{nu{m}_1}(x_i-u)(x_i-u{)}^T$$

最直观的思路是对Y点云也进行如下求解，然后对$u_1^x$,$u_1^y$,${\sum }_1^x$，${\sum }_1^y$做联合概率计算，但是实际上不是这样处理，在求出第一个栅格内$u_1^x$和${\sum }_1^x$后，直接用Y点云内第一个栅格的点云求解概率公式 如下
$$f(X,y_i^{{}^{\prime }})=\frac{1}{\sqrt{2\pi }\sqrt{\left|\sum \right|}}exp(-\frac{(y_i^{{}^{\prime }}-u_1^x{)}^T{\sum }^{-1}(y_i^{{}^{\prime }}-u_1^x)}{2})$$
上边的公式是一个激光点的，那么对于第一个栅格的联合概率就是把$nu{m}_1$ 个点的$f(X,y_i^{{}^{\prime }})$乘在一起，那么在把27个栅格的概率乘在一起，最终联合概率为
$$\Psi =\prod _{i=1}^{N}f(X,y_i^{{}^{\prime }})=\prod _{i=1}^N\frac{1}{\sqrt{2\pi }\sqrt{\left|\sum \right|}}exp(-\frac{(y_i^{{}^{\prime }}-u_1^x{)}^T{\sum }^{-1}(y_i^{{}^{\prime }}-u_1^x)}{2})$$

$$max(\Psi =min(\sum _{i=1}^N{y}_i^{{}^{\prime }}-u_j^x{)}^T\sum _j^{-1}(y_i^{{}^{\prime }}-u_j^x)$$
$u_j^x$,${\sum }_j^{-1}$表示点云$y_i^{{}^{\prime }}$所在栅格j对应的均值和方差，这个等式省略了中间推导过程。
求解推导
最终得到Loss模型为：
目标函数$min({\sum }_{i=1}^N{y}_i^{{}^{\prime }}-u_j^x{)}^T{\sum }_j^{-1}(y_i^{{}^{\prime }}-u_j^x)$，求解合适的RT使得loss模型最小。
其中： $y_i^{{}^{\prime }}=R{y}_i+t$ 将Y点云通过RT变换后的对应位置
从loss函数和ICP的Loss函数对比，可以看出NDT的计算量小于ICP系列，因为NDP只需要遍历一个Y点云的所有激光点，X点云用u和$\sum$的形式描述。
$e_i(p)=y_i^{{}^{\prime }}-u$
$F_i(p)=e_i^T(p){\sum }^{-1}{e}_i(p)$
$min({\sum }_{i=1}^N{y}_i^{{}^{\prime }}-u_j^x{)}^T{\sum }_j^{-1}(y_i^{{}^{\prime }}-u_j^x)=min{\sum }_{i=1}^N{F}_i(p)$
采取迭代优化的方法
$$min\sum _{i=1}^N{F}_i(p+\Delta p)=min(\sum _{i=1}^N{e}_i^T(p+\Delta p)\sum ^{-1}{e}_i(p+\Delta p))$$
$$F_i(p+\Delta p)=F_i(p)+2b_i^T\Delta p+\Delta p^T{H}_i\Delta p$$
其中$b_i^T=e_i^T{\sum }^{-1}{J}_i$ ,$H_i=J_i^T{\sum }^{-1}{J}_i$

迭代优化的更新值为：
$$\Delta F_i(p)=F_i(p+\Delta p)-F_i(p)=2b_i^T\Delta p+\Delta p^T{H}_i\Delta p$$

5.参考

参考：深蓝学院任乾老师课程