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

1.本篇大纲

2.前端里程计综述

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

3.直接匹配的ICP系列

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

3）模型推导
下面以其中点到点+SVD分解求解为例，进行模型建立和推导
loss模型：
①E(R,T)=1N∑i=1N∣∣xi−Ryi−T∣∣2① E(R,T) = \frac{1}{N} \sum_{i =1}^{N}||x_{i}-Ry_{i}-T||^{2} ①E(R,T)=N1i=1∑N∣∣xi−Ryi−T∣∣2
X=x1,x2,...,xNX = {x_{1},x_{2},...,x_{N}}X=x1,x2,...,xNY=y1,y2,...,yNY = {y_{1},y_{2},...,y_{N}}Y=y1,y2,...,yN
其中N表示点云预处理后点云个数，X,Y为前后两个关键帧的点云，其中假设x1和y1为最邻近点
Loss模型的思路：是找一个位姿变换关系（用R，T体现）使两个点云中各对对应点尽可能重合，也就是让E（R，T）最小。
求解推导：
②E(R,T)=1N∑i=1N∣∣xi−Ryi−T−ux+Ruy+ux−Ry∣∣2②E(R,T) = \frac{1}{N} \sum_{i =1}^{N}||x_{i}-Ry_{i}-T-u_{x}+Ru_{y}+u_x-R_{y}||^{2} ②E(R,T)=N1i=1∑N∣∣xi−Ryi−T−ux+Ruy+ux−Ry∣∣2
③E(R,T)=1N∑i=1N(∣∣xi−ux−R(yi−uy)∣∣2+∣∣ux−Ruy−T∣∣2)③E(R,T) = \frac{1}{N} \sum_{i =1}^{N}(||x_{i}-u_{x}-R(y_{i}-u_{y})||^{2}+||u_{x}-Ru_{y}-T||^{2})③E(R,T)=N1i=1∑N(∣∣xi−ux−R(yi−uy)∣∣2+∣∣ux−Ruy−T∣∣2)

由①到②是一步构造，目的就是为了转化成③，展开说说：
其中uxu_{x}ux表示X点云的均值，直观理解就是中心，最后转化成③的形式可以看出来，Loss模型分为前后两部分，前部分只与R有关，后部分当已知R总可以找到一个T使该部分为0，所以整体思路变成先求解前部分求出R，然后用已知R令后部分为0求出T。
③中令xi′=xi−uxx_{i}^{'} = x_i-u_xxi′=xi−ux 表示“去除”自身中心后的点云，这个x1′x_{1}^{'}x1′可以求出来
可以得到：④E1(R,T)=1N∑i=1N∣∣xi′−R(yi′)∣∣2④E_1(R,T) =\frac{1}{N} \sum_{i =1}^{N}||x_{i}^{'}-R(y_{i}^{'})||^{2}④E1(R,T)=N1i=1∑N∣∣xi′−R(yi′)∣∣2
至此这个问题被完全转化为数学问题，剩下的公式推导基本就是SVD分解的部分，只写出公式不做讲解
min(E1(R,T))=min(1N∑i=1N∣∣xi′−R(yi′)∣∣2)=min(1N∑i=1N(xi′Txi′−yi′TRTRyi′−2xiT′Ryi′))=max(1N∑i=1N(2xiT′Ryi′))=max(∑i=1N(xiT′Ryi′))=max(∑i=1NTrace(xiT′Ryi′))=max(∑i=1NTrace(Ryi′xiT′))=max(Trace(RH))H=∑i=1Nyi′ximin(E_1(R,T)) =min(\frac{1}{N} \sum_{i =1}^{N}||x_{i}^{'}-R(y_{i}^{'})||^{2}) \newline=min(\frac{1}{N} \sum_{i =1}^{N}(x_{i}^{'T}x_{i}^{'}-y_{i}^{'T}R^TRy_{i}^{'}-2x_i^{T'}Ry_i^{'})) \newline=max(\frac{1}{N} \sum_{i =1}^{N}(2x_i^{T'}Ry_i^{'})) \newline = max(\sum_{i =1}^{N}(x_i^{T'}Ry_i^{'})) \newline = max(\sum_{i =1}^{N}Trace(x_i^{T'}Ry_i^{'})) \newline = max(\sum_{i =1}^{N}Trace(Ry_i^{'}x_i^{T'})) \newline= max(Trace(RH)) \newline H = \sum_{i =1}^{N}y_i^{'}x_imin(E1(R,T))=min(N1i=1∑N∣∣xi′−R(yi′)∣∣2)=min(N1i=1∑N(xi′Txi′−yi′TRTRyi′−2xiT′Ryi′))=max(N1i=1∑N(2xiT′Ryi′))=max(i=1∑N(xiT′Ryi′))=max(i=1∑NTrace(xiT′Ryi′))=max(i=1∑NTrace(Ryi′xiT′))=max(Trace(RH))H=i=1∑Nyi′xi
至此问题转化为找到一个R使得Trace(RH)最大，那其实就是矩阵分析的内容，就是让RH可以分解成ATAA^TAATA的形式，这部分就不写了
最后 T=ux−RuyT = u_{x}-Ru_{y}T=ux−Ruy

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=x1,x2,...,xNX = {x_{1},x_{2},...,x_{N}}X=x1,x2,...,xN
Y=y1,y2,...,yNY = {y_{1},y_{2},...,y_{N}}Y=y1,y2,...,yN
NDT的推导部分主要是方便理解思路，忽略数学概率部分的推导。
划分栅格的目的是将点云分布用正态分布的形式进行描述，假设将点云X分为一个3X3X3的栅格（像一个最简单的魔方），那么假设第一个栅格内点云个数为num1num_1num1,注意∑j=127(numi)=N\sum_{j=1}^{27}( {num_i})=N∑j=127(numi)=N,在第一个栅格内，进行均值和协方差求解
u1=1numi∑i=1num1xiu_1 = \frac{1}{num_i}\sum_{i=1}^{num_1}x_iu1=numi1i=1∑num1xi
∑1=1num1−1∑i=1num1(xi−u)(xi−u)T{\sum}_1 = \frac{1}{num_1-1}\sum_{i=1}^{num_1}(x_i-u)(x_i-u)^T∑1=num1−11i=1∑num1(xi−u)(xi−u)T

最直观的思路是对Y点云也进行如下求解，然后对u1xu_1^xu1x,u1yu_1^yu1y,∑1x{\sum}_1^x∑1x，∑1y{\sum}_1^y∑1y做联合概率计算，但是实际上不是这样处理，在求出第一个栅格内u1xu_1^xu1x和∑1x{\sum}_1^x∑1x后，直接用Y点云内第一个栅格的点云求解概率公式如下
f(X,yi′)=12π∣∑∣exp(−(yi′−u1x)T∑−1(yi′−u1x)2)f(X,y_i^{'}) = \frac{1}{\sqrt{2\pi }\sqrt{\left | \sum \right |}}exp(-\frac{(y_i^{'}-u_1^x)^T\sum ^{-1}(y_i^{'}-u_1^x)}{2})f(X,yi′)=2π∣∑∣1exp(−2(yi′−u1x)T∑−1(yi′−u1x))
上边的公式是一个激光点的，那么对于第一个栅格的联合概率就是把num1num_1num1 个点的f(X,yi′)f(X,y_i^{'})f(X,yi′)乘在一起，那么在把27个栅格的概率乘在一起，最终联合概率为
Ψ=∏i=1Nf(X,yi′)=∏i=1N12π∣∑∣exp(−(yi′−u1x)T∑−1(yi′−u1x)2)\Psi = \prod_{i=1}^{N}f(X,y_i^{'}) = \prod_{i=1}^{N}\frac{1}{\sqrt{2\pi }\sqrt{\left | \sum \right |}}exp(-\frac{(y_i^{'}-u_1^x)^T\sum ^{-1}(y_i^{'}-u_1^x)}{2})Ψ=i=1∏Nf(X,yi′)=i=1∏N2π∣∑∣1exp(−2(yi′−u1x)T∑−1(yi′−u1x))

max(Ψ=min(∑i=1Nyi′−ujx)T∑j−1(yi′−ujx)max(\Psi= min(\sum _{i=1}^{N}y_i^{'}-u_j^x)^T\sum^{-1}_j(y_i^{'}-u_j^x)max(Ψ=min(i=1∑Nyi′−ujx)Tj∑−1(yi′−ujx)
ujxu_j^xujx,∑j−1\sum^{-1}_j∑j−1表示点云yi′y_i^{'}yi′所在栅格j对应的均值和方差，这个等式省略了中间推导过程。
求解推导
最终得到Loss模型为：
目标函数min(∑i=1Nyi′−ujx)T∑j−1(yi′−ujx)min(\sum _{i=1}^{N}y_i^{'}-u_j^x)^T\sum^{-1}_j(y_i^{'}-u_j^x)min(∑i=1Nyi′−ujx)T∑j−1(yi′−ujx)，求解合适的RT使得loss模型最小。
其中： yi′=Ryi+ty_i^{'} = Ry_i+tyi′=Ryi+t 将Y点云通过RT变换后的对应位置
从loss函数和ICP的Loss函数对比，可以看出NDT的计算量小于ICP系列，因为NDP只需要遍历一个Y点云的所有激光点，X点云用u和∑\sum∑的形式描述。
ei(p)=yi′−ue_i(p) = y_i^{'}-uei(p)=yi′−u
Fi(p)=eiT(p)∑−1ei(p)F_i(p) = e_i^T(p)\sum^{-1} e_i(p)Fi(p)=eiT(p)∑−1ei(p)
min(∑i=1Nyi′−ujx)T∑j−1(yi′−ujx)=min∑i=1NFi(p)min(\sum _{i=1}^{N}y_i^{'}-u_j^x)^T\sum^{-1}_j(y_i^{'}-u_j^x)= min\sum _{i=1}^{N}F_i(p)min(∑i=1Nyi′−ujx)T∑j−1(yi′−ujx)=min∑i=1NFi(p)
采取迭代优化的方法
min∑i=1NFi(p+Δp)=min(∑i=1NeiT(p+Δp)∑−1ei(p+Δ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))mini=1∑NFi(p+Δp)=min(i=1∑NeiT(p+Δp)∑−1ei(p+Δp))
Fi(p+Δp)=Fi(p)+2biTΔp+ΔpTHiΔpF_i(p+\Delta p)=F_i(p)+2b_i^T\Delta p+\Delta p^TH_i\Delta pFi(p+Δp)=Fi(p)+2biTΔp+ΔpTHiΔp
其中biT=eiT∑−1Jib_i^T = e_i^T\sum^{-1}J_ibiT=eiT∑−1Ji ,Hi=JiT∑−1JiH_i = J_i^T\sum^{-1}J_iHi=JiT∑−1Ji

迭代优化的更新值为：
ΔFi(p)=Fi(p+Δp)−Fi(p)=2biTΔp+ΔpTHiΔp\Delta F_i(p) = F_i(p+\Delta p)- F_i(p)=2b_i^T\Delta p+\Delta p^TH_i\Delta pΔFi(p)=Fi(p+Δp)−Fi(p)=2biTΔp+ΔpTHiΔp

5.参考

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