Simple-LIO-SAM——（九）点云匹配算法详解
- ⭐ Zeal's Blog
- 🛠 知乎专栏
- 🌀 项目仓库
前言
LIOSAM中激光历程计的点云匹配方法沿用自LOAM，这个基于点到线和点到面距离求解最小二乘问题的方法起始被用到很多框架，包括LOAM,LOAM,Lego-LOAM,各种LOAM的变种，LIOSAM及其变种。这段点云匹配代码可以说被重用了很多次，但是各个论文对这部分的描述实际上很少或者基本省略，LOAM原始论文对这部分的描述与实际的代码实现差别比较大，导致如果想真正看懂这段代码还是挺难的。这篇文章详细地剖析这部分的原理、实现、以及与论文中不同之处。这里面唯一有一点还没有搞清楚的是对于 高斯矩阵退化部分的实现。
由于本文主要是想讲明白在上述这些SLAM框架中点云匹配的实现，因此对于最小二乘问题的求解不会详细展开，需要这部分知识可以参考最小二乘问题求解的四种解法或者《视觉SLAM十四讲》中关于最小二乘问题求解的章节。
点云-局部地图匹配流程Scan-2-Map
在LIOSAM中，对于每一帧点云\(\mathbb{F}_i\)会先进行特征点提取。特征点包括线特征点和平面特征点（具体方法参考本博客文章：Simple-LIO-SAM——（六）特征提取模块)。我们记提取后的线特征点云为\(F^e_i\)，平面特征点云为\(F^p_i\)。
经过一段时间后\(1,2,3,...,i-1\)后，我们拥有一系列关键帧的线特征点云\(F^e_1,F^e_2,F^e_3,...,F^e_{i-1}\)和平面特征点云\(F^p_1,F^p_2,F^p_3,...,F^p_{i-1}\)，以及这些关键帧对应的位姿\(T_1,T_2,T_3,...,T_{i-1}\). 同时对于点云\(\mathbb{F}_i\)，我们通过IMU里程计还可以获得该帧点云的初始位姿估计\(\hat{T}_i\)，这个位姿估计\(\hat{T}_i\)其实就是我们要优化的量。
在LIOSAM中，激光里程计匹配的是当前帧点云和局部地图。局部地图是通过当前帧空间以及时间近邻的其他关键帧构建的。 
对于任何一帧点云以及对应的位姿，我们可以将点云转换到地图坐标系： \[\mathbb{F}^m_i = R_i\mathbb{F}_i+t_i\] 其中\(m\)上标指的是地图坐标系，\(R_i\)和\(t_i\)是位姿\(T_i\)中的旋转矩阵和平移向量。
对选择为构建局部地图的关键帧分别应用上式并累加起来（实际累加后会进行体素降采样）后就构成局部地图\(M^e_i\)和\(M^p_i\)，\(e\)和\(p\)下标表示edge和planner。
同时，我们对当前帧的特征点云\(F^e_i\)和\(F^p_i\)应用位姿的初始估计\(\hat{T}_i\)可以得到在近似地图坐标系下的点云。为了方便后面叙述，这里省略坐标系和关键帧索引的上下标。重申一下我们的输入：
- 地图坐标系下的局部点云地图\(M^e\)和\(M^p\)
- 雷达坐标系下的当前帧特征点云\(F^e\)和\(F^p\)
- 当前帧的初始位姿估计\(\hat{T}_i=[\hat{R}_i,\hat{t}_i]\)
我们的目标就是优化位姿\(T_i\)使得用这个位姿将\(F^e\)和\(F^p\)转换到地图坐标系后与地图点云的匹配程度最好。
几何相关知识
点到线距离
论文中计算方法
上图的子图（a）展示如何对当前帧边缘点点集\(F^e\)中的每一个点从局部地图\(M^e\)中找到对应的直线。对于每一个边缘点\(P^{F^e}_i \in F^e\)，从局部地图的边缘点集合\(M^e\)中找到最近点\(P^{M^e}_j\)，橘色线是点\(j\)所在的激光线束，蓝色线是前后相近的另外两条激光线束。从前后两条激光线束（蓝色线）找出与\(e_i\)距离最近的点，并选择两者中距离更小的点作为点\(P^{M^e}_l\)，则经过点\(P^{M^e}_j\)、\(P^{M^e}_l\)组成的线为点\(P^{F^e}_i\)对应的直线。那么问题就转化为点\(P^{F^e}_i\)到经过点\(P^{M^e}_j\)、\(P^{M^e}_l\)的直线的距离。
该距离计算公式如下：
\[ d_{e}=\frac{|(P^{F^e}_i - P^{M^e}_j)\times (P^{F^e}_i - P^{M^e}_l)|}{|(P^{M^e}_j - P^{M^e}_l)|}\tag{1} \]
公式（1）中分子部分计算的是向量\((P^{F^e}_i - P^{M^e}_j)\)和\((P^{F^e}_i - P^{M^e}_l)\)的叉积的模长。两个向量的叉积是一个向量，方向于两个向量构成的平面垂直，模长等于两个向量组成的平行四边形面积。公式（1）的分母代表的是该平行四边形对角线的长度。因此分子除以分母就等于直线外一点到直线的距离。
代码中计算方法
在论文中对\(F^e\)的点找到\(M^e\)对应的直线是通过找到两个距离最近的点，但是在代码中是找到距离最近的5个点，然后计算这5个点的协方差，对协方差做特征值分解，最大的特征值对应的特征向量为主方向，并判断这5个点的分布是否接近直线的要求。 代码中寻找点到局部地图的对应直线，然后计算距离和法向量主要是在mapOptimization中的函数cornerOptimization中完成。
- 对点集\(F^e\)中的点从局部地图\(M^e\)中找到最近5个点
1 | // 从局部地图（已经提前设置好kdtree）中找到最近的5个点 |
- 计算这5个点的协方差矩阵
1 | // cx,cy,cz是检索出的5个点的中心坐标 |
- 计算直线方向，也就是特征值分解，最大特征值对应的特征向量为数据主方向
1 | // 对协方差矩阵做特征值分解，最大特征值对应的特征向量是这5个点的主方向 |
- 计算点到直线距离
1 | // 以下部分是在计算当前点pointSel到检索出的直线的距离和方向，如果距离够近，则认为匹配成功，否则认为匹配失败 |
点到面距离
论文中计算方法
再次贴出论文中的图
与计算点到直线的距离类似，对于一个平面点\(P^{F^p}_i \in F^p\)，从局部地图平面点集\(M^p\)中找到最近点\(P^{M^p}_j\)，再从该点所在的激光线束找到另一个点\(P^{M^p}_l\)以及前后两条激光线束中找到另外一个最近点\(P^{M^p}_m\)，这样就确保了三个点不会共线。 那么问题就转化为已知经过三个点\(P^{M^p}_{jlm}\)的平面，求平面外一点\(P^{F^p}_i\)到该平面的距离 首先，根据三个点，我们可以计算该平面的法向量，并归一化为单位法向量
\[ n_{jlm}=(P^{M^p}_j - P^{M^p}_l)\times (P^{M^p}_j - P^{M^p}_m) \]
\[ n_{jlm} = \frac{n_{jlm}}{|n_{jlm}|} \]
于是，点到平面的距离为 \[ d_p = |(P^{F^p}_i - P^{M^p}_j)n_{jlm}|\tag{2} \]
代码中计算方法
在代码中，也是先根据点\(P^{F^p}_i\)从局部地图平面点集中找到最近的5个点，然后对这5个点用最小二乘法（求解超定方程）得到拟合平面的法向量。得到平面方程后就可以直接计算点到平面的距离了。这部分代码在mapOptimization.cpp中surfOptimization函数
- 从局部地图找到距离最近的5个点
1 | // 与边缘点找直线一样，从局部地图的平面点集中找到与pointSel距离最近的5个点 |
- 求解方程Ax+By+Cz+1=0方程
1 | // 下面的过程要求解Ax+By+Cz+1=0的平面方程 |
- 求点到平面距离
1 | float pa = matX0(0, 0); |
最小二乘问题求解
从前面两步，我们得到了一些边缘点和平面点到局部地图的距离和距离向量，我们的目标就是优化这些距离。由于我们现在有6个未知数，也就是位姿的6个自由度\(T_i=[t_x,t_y,t_z,roll,pitch,yaw]\)，但是我们的点超过6个，因此是求解最小二乘问题。
最小二乘问题是最优化里的基础问题，这里就不展开公式推导了，具体可以参考这篇文章最小二乘问题求解的四种解法。
常见的求解最小二乘问题有高斯牛顿法（Gauss-Newton Method）和列温伯格马夸克法（Levernberg-Marquate），后者是前者的改进。LOAM论文里面虽然用的是LM算法，但是在代码中实际用的是GN算法。这里只强调GN算法的核心思路。
GN算法也是迭代更新的算法，每一步重点在与求出未知量的更新方向和步长，每一步将更行向量叠加到未知量上，让目标函数逐渐收敛。 在这里，未知量是位姿\(T_i\)，目标函数是点到直线点到平面距离\(\mathbb{d}\)，因此，我们在每一步迭代中要找到一个\(\Delta{T}_i\)，更新未知量\(T_i = T_i + \Delta{T}_i\)，并使得距离\(\mathbb{b}\)逐渐下降。
在高斯牛顿法中，更新向量的是通过求解增量方程得到
\[ J(x)J(x)^T\Delta{x}=-J(x)f(x)\tag{3} \]
公式（3）就是高斯牛顿法的增量方程，\(J(x)\)是雅克比矩阵，也就是目标函数相对于未知量的偏导；\(f(x)\)是目标函数；\(\Delta{x}\)就是我们要求的增量。
代码中点云匹配算法（基于GN算法）
这部分内容对于数学公式推导部分主要参考wykxwyc.github.io，但是原文中对于旋转矩阵对欧拉角的求导似乎有点问题，与代码对不上。
欧拉角转旋转矩阵
代码中对角度的表达使用欧拉角表达，因此我们需要先将欧拉角转换为旋转矩阵形式。记\(x,y,z\)轴的角度为\(\alpha,\beta,\gamma\)
\[ \begin{split} R_{\alpha}= \begin{bmatrix} 1&0&0\\ 0&\cos\alpha&-\sin\alpha\\ 0&\sin\alpha&\cos\alpha \end{bmatrix} \end{split} \]
\[ \begin{split} R_{\beta}= \begin{bmatrix} \cos\beta&0&\sin\beta\\ 0&1&0\\ -\sin\beta&0&\cos\beta \end{bmatrix} \end{split} \]
\[ \begin{split} R_{\gamma}= \begin{bmatrix} \cos\gamma&-\sin\gamma&0\\ \sin\gamma&\cos\gamma&0\\ 0&0&1\\ \end{bmatrix} \end{split} \]
PS: 为了简便起见，我们在后面的推导中使用\(c_1,c_2,c_3\)表示\(\cos\alpha,\cos\beta,\cos\gamma\)，用\(s_1,s_2,s_3\)表示\(\sin\alpha,\sin\beta,\sin\gamma\)
根据上面转换关系，使用\(Z-X-Y\)顺序，将欧拉角转换为旋转矩阵如下
\[ \begin{split} \begin{array}{rcl} R &=&R_yR_xR_z\\ &=& \begin{bmatrix} c_2&0&s_2\\ 0&1&0\\ -s_2&0&c_2 \end{bmatrix} \begin{bmatrix} 1&0&0\\ 0&c_1&-s_1\\ 0&s_1&c_1 \end{bmatrix} \begin{bmatrix} c_3&-s_3&0\\ s_3&c_3&0\\ 0&0&1 \end{bmatrix}\\ &=& \begin{bmatrix} c_2c_3+s_1s_2s_3&-c_2s_3+s_1s_2c_3&c_1s_2\\ c_1s_3&c_1c_3&-s_1\\ -s_2c_3+s_1c_2s_3&s_2s_3+s_1c_2c_3&c_1c_2 \end{bmatrix} \end{array} \end{split} \]
旋转矩阵对欧拉角求偏导
有了上面欧拉角转旋转矩阵的公式之后，求旋转矩阵相对于欧拉角的偏导就容易多了，先求\(R\)相对于角\(\alpha\)的偏导
\[ \begin{split} \frac{\partial R}{\partial \alpha} = \begin{bmatrix} c_1s_2s_3&c_1s_2c_3&-s_1s_2\\ -s_1s_3&-s_1c_3&-c_1\\ c_1c_2s_3&c_1c_2c_3&-s_1c_2 \end{bmatrix} \end{split} \]
再求\(R\)相对于\(\beta\)的偏导
\[ \begin{split} \frac{\partial R}{\partial \beta} = \begin{bmatrix} -s_2c_3+s_1c_2s_3&s_2s_3+s_1c_2c_3&c_1c_2\\ 0&0&0\\ -c_2c_3-s_1s_2s_3&c_2s_3-s_1s_2c_3&-c_1s_2 \end{bmatrix} \end{split} \]
最后求\(R\)相对于\(\gamma\)的偏导
\[ \begin{split} \frac{\partial R}{\partial \gamma} = \begin{bmatrix} -c_2s_3+s_1s_2c_3&-c_2c_3-s_1s_2s_3&0\\ c_1c_3&-c_1s_3&0\\ s_2s_3+s_1c_2c_3&s_2c_3-s_1c_2s_3&0 \end{bmatrix} \end{split} \]
目标函数对位姿求雅克比矩阵
时刻牢记：位姿\(T_i=[t_x,t_y,t_z,\alpha,\beta,\gamma]=[R,t]\)是我们要求的量
对于当前帧特征点\(F^e,F^p\)中的一个点\(P_i=[p_x,p_y,p_z]\)（这里没有区分边缘点和平面点，因为两者的偏导公式一样），我们使用函数\(G(\cdot)\)将其从雷达坐标系转换到局部坐标系(m上标表示地图坐标系)
\[ P^m_i=G(P_i,T_i)=R\cdot P_i+t \]
我们使用函数\(D(\cdot)\)表示该点到局部地图的距离，也就是目标函数
\[ loss=D(G(P_i,T_i),Map) = d \]
上式中\(d\)对于边缘点代表点到直线距离，对于平面点表示点到平面距离
误差对旋转求偏导
这里以\(x\)轴的旋转\(\alpha\)为例
\[ \begin{split} \begin{array}{rcl} \frac{\partial loss}{\partial \alpha} &=& \frac{\partial D\left(G(P_i,T_i),Map\right)}{\partial \alpha}\\ &=& \frac{\partial D}{\partial G}\cdot \frac{\partial G}{\partial \alpha}\\ &=& \frac{\partial D}{\partial G}\cdot \frac{\partial R\cdot P_i+t}{\partial \alpha}\\ &=& \frac{\partial D}{\partial G}\cdot \frac{\partial R\cdot P_i}{\partial \alpha}\\ &=& \frac{\partial D}{\partial G}\cdot \frac{\partial R}{\partial \alpha}\cdot P_i \end{array} \end{split}\tag{4} \]
误差对平移求偏导
这里以\(x\)轴的平移\(t_x\)为例
\[ \begin{split} \begin{array}{rcl} \frac{\partial loss}{\partial t_x} &=& \frac{\partial D\left(G(P_i,T_i),Map\right)}{\partial t_x}\\ &=& \frac{\partial D}{\partial G}\cdot \frac{\partial G}{\partial t_x}\\ &=& \frac{\partial D}{\partial G}\cdot \frac{\partial R\cdot P_i+t}{\partial t_x}\\ &=& \frac{\partial D}{\partial G}\cdot \frac{\partial R\cdot P_i}{\partial \alpha} + \frac{\partial D}{\partial G}\cdot\frac{\partial t}{\partial t_x}\\ &=& 0+\frac{\partial D}{\partial G}\cdot\frac{\partial t}{\partial t_x}\\ &=& \frac{\partial D}{\partial G} \end{array} \end{split}\tag{5} \]
求解\(\frac{\partial D}{\partial G}\)
在上面误差对旋转求偏导中旋转矩阵对欧拉角求偏导\(\frac{\partial R}{\partial \alpha}\)已经在前面的章节推导过了。公式(4)(5)还有一个未知量就是\(\frac{\partial D}{\partial G}\)
\[ \frac{\partial D}{\partial G} = \frac{\partial d}{\partial P^m_i} \]
也就是说\(\frac{\partial D}{\partial G}\)求的是损失函数也就是点到线和点到面的距离相对于点（地图坐标系下）的偏导数。这点可以理解成求一个移动方向，使得让点\(P^m_i\)沿着这个方向移动，损失函数上升得最快。很容易想到这个方向就是法线方向（对于点到直线的情况则是直线的垂线方向） 因此
\[ \frac{\partial D}{\partial G} = \frac{\partial d}{\partial P^m_i}=\big(\frac{\partial d}{\partial x}, \frac{\partial d}{\partial y}, \frac{\partial d}{\partial z}\big)=(n_x,n_y,n_z) \]
代码中雅克比矩阵的计算
这部分计算主要在mapOptimization.cpp中的LMOptimization函数。这里要注意的是，LIOSAM这部分代码计算中还加了坐标系的转换，实际上是没有必要的，这部分代码可以看LeGO-LOAM的代码，是完全一样的
1 | // 计算三轴欧拉角的sin、cos，后面使用旋转矩阵对欧拉角求导中会使用到 |
更新
得到雅克比矩阵后就可以直接求解高斯-牛顿法里的增量方程
\[ J(x)J(x)^T\Delta{x}=-J(x)f(x)\tag{3} \]
1 | cv::transpose(matA, matAt); |
高斯矩阵退化
这部分代码其实有一小段没办法完全理解，就是在进行第一次迭代时会对近似Hessian矩阵做是否退化的的判断，然后对退化的方向进行处理，最后对增量进行重新加权。但是实在没有找到这种用法的出处。
1 | // 如果是第一次迭代，判断求解出来的近似Hessian矩阵，也就是J^{T}J:=matAtA是否退化 |
收敛条件判断
当增量步长达到一定阈值后，认为优化已经收敛，因此可以跳出后续迭代。这里用增量中的角度增量和平移增量的幅度做判断。
1 | // 计算roll、pitch、yaw的迭代步长 |