[笔记]三维激光SLAM学习——LiDAR里程计原理推导&代码实现

[笔记]三维激光SLAM学习——LiDAR里程计原理推导&代码实现

前言

最近看回来之前发的博客，发现了埋了很多坑，其中也有很多错误的地方，所以现在重新写一遍，一个个坑补回来。

一、LiDAR里程计原理

简述：目前用得比较多的还是LOAM中提出的方法，以一个粗糙度$c$值来区分边缘和平面，然后边缘点、平面点分别与边缘线、平面匹配，从而得到一个两帧之间的LiDAR位姿。

简单地说，就是提取锐利边缘特征点和平面特征点，并分别将特征点与边缘线段和平面曲面片进行匹配。

参考文献：Zhang J , Singh S . Low-drift and Real-time Lidar Odometry and Mapping[J]. Autonomous Robots, 2017, 41(2):401-416.

1、坐标系定义

• 激光雷达坐标系$L$是3D坐标系，其原点位于激光雷达的几何中心。 $x$轴指向左侧，$y$轴指向上方，$z$轴指向前方。
• 世界坐标系$W$是在初始位置与$L$一致的3D坐标系。

2、特征点提取

LOAM论文中选择在锐利边缘和平面/曲面的特征点。设$i$为$P_k$中的一个点，$i\in P_k$，设$S$为LiDAR在同一扫描中返回的$i$的连续点集。定义一个“粗糙度”$c$来评估局部曲面的平滑度，用于边缘特征点、平面特征点和普通点的分类。
（这个粗糙度可以粗暴的理解为点$i$到邻点的距离之和）
$$c=\frac{1}{\left|\mathbf{S}\right|\cdot \Vert {\mathbf{X}}_{\left(k,i\right)}^L\Vert }\Vert \sum _{j\in \mathbf{S},j\ne i}({\mathbf{X}}_{(k,i)}^{\mathbf{L}}-{\mathbf{X}}_{(k,j)}^{\mathbf{L}})\Vert$$

• 扫描中的点根据$c$值进行排序，然后用最大$c$值（即边缘点）和最小$c$值（即平面点）选择特征点。
• 一次扫描分为四个均等的区域。每个区域中最多可提供2个边缘点和4个平面点。

两点约束：

• 不能位于与激光束大致平行的曲面片上
• 不能位于遮挡区域的边界上
  
  因为这些点通常被认为是不可靠的。

3、特征点匹配

设$t_k$为扫描$k$的开始时间，在开始时，$P_k$是一个空集，在扫描过程中随着接收到更多点而增加。在每次扫描结束时，在扫描过程中感知到的点云$P_k$重新投影到时间戳$t_{k+1}$，如上图所示。然后将重新投影的点云表示为${\overline{P}}_k$。在下一个扫描$k+1$期间，${\overline{P}}_k$与新接收的点云$P_{k+1}$一起估计激光雷达的运动。
假设${\overline{P}}_k$和$P_{k+1}$现在都可用，并开始寻找两个激光雷达点云之间的对应关系。从$P_{k+1}$中找到边缘特征点和平面特征点，设$E_{k+1}$和$H_{k+1}$分别为边缘特征点和平面特征点的集合。我们将从${\overline{P}}_k$中找边缘线作为$E_{k+1}$中的边缘特征点对应，以及从${\overline{P}}_k$中找平面块作为$H_{k+1}$中的平面特征点对应。
在扫描$k+1$开始时，$P_{k+1}$是一个空集，在扫描过程中随着接收到更多点而增加。激光雷达里程计递归地估计扫描过程中的6自由度运动，并随着$P_{k+1}$的增加逐渐包含更多的点。在每次迭代中，使用当前估计的变换，将$E_{k+1}$和$H_{k+1}$重新投影到扫描的开始。将重新投影的点集设为${\tilde{E}}_{k+1}$和${\tilde{H}}_{k+1}$。对于${\tilde{E}}_{k+1}$和${\tilde{H}}_{k+1}$中的每个点，我们将在${\overline{P}}_k$中找到最近的相邻点。这里，${\overline{P}}_k$存储在3D KDtree中，用于快速索引。

边缘特征点与边缘线对应
上图（a）表示寻找边缘线作为边缘特征点的对应关系的过程。设$i$为${\tilde{E}}_{k+1}，i\in {\tilde{E}}_{k+1}$中的点。边缘线由两点表示。设$j$为$i$在${\overline{P}}_k$中的最近邻，$j\in {\overline{P}}_k$，设$l$为$i$在连续两次扫描中$i$的最近邻。$(j，l)$形成了$i$的对应关系。然后，为了验证$j$和$l$都是边缘点，我们根据$c$值检查局部表面的光滑度。在这里，我们特别要求$j$和$l$来自不同的扫描，因为单个扫描不能包含来自同一边缘线的多个特征点。边缘线在扫描平面上只有一个例外。但是，如果是这样，则边缘线将退化并在扫描平面上显示为直线，并且边缘线的特征点不应首先提取。
平面特征点与平面曲面块对应
上图（b）表示寻找平面曲面块作为平面特征点的对应关系的过程。设$i$为${\tilde{H}}_{k+1}$中的点，$i\in {\tilde{H}}_{k+1}$。平面斑块由三个点表示。与上一部分相似，我们在${\overline{P}}_k$找到$i$的最近邻，表示为$j$。然后，我们找到另外两个点$l$和$m$，它们是$i$的最近邻，一个点与$j$在同一扫描中，另一个在两次连续扫描中直到$j$的扫描中。这保证了这三个点是非共线的。为了验证$j$，$l$和$m$均为平面点，我们基于$c$值再次检查局部表面的光滑度。

3.1、计算对应特征的距离

利用找到的特征点的对应关系，现在我们导出表达式以计算从特征点到其对应关系的距离。下面将通过最小化特征点的总距离来估计激光雷达运动。
边缘特征点距离计算：
对于点$i\in {\tilde{E}}_{k+1}$，如果$(j，l)$是相应的边缘线$j，l\in {\overline{P}}_k$，则点到线的距离可以计算为

其中，${\tilde{X}}_{(k+1,i)}^{\mathbf{L}}$，${\overline{X}}_{(k,j)}^{\mathbf{L}}$和${\overline{X}}_{(k,l)}^{\mathbf{L}}$分别是$L$中点$i$，$j$和$l$的坐标。
直观理解：公式的分子是两个向量的叉乘，而叉乘后求模就变成了求两个向量构成的三角形的面积。公式的分母是向量的模相当于三角形底边的长。三角形面积除以一条边就可以求出该边到对应顶点的距离，也就是边角点到边角线的距离。直观上的理解如下图所示：

平面特征点距离计算:
对于点$i\in {\tilde{H}}_{k+1}$，如果$(j，l，m)$是对应的平面曲面块$j，l，m\in {\overline{P}}_k$，则点到平面的距离为

其中，${\overline{X}}_{(k,m)}^{\mathbf{L}}$是$L$中点$m$的坐标。
直观理解：公式的分子包括两部分，上边是获得一个向量，下边也是获得一个向量，但两个向量叉乘再取模就表示的求下边得到三角形面积，上面表示立方体的高，两者相乘就表示立方体体积。而分母表示立方体底面三角形的面积，得到的高就是平面点到平面的距离。直观上的理解如下图所示:

4 、用非线性优化方法进行运动估计

回想一下，上式计算${\tilde{E}}_{k+1}$和${\tilde{H}}_{k+1}$中的点之间的距离及其对应关系。结合边缘特征点距离计算式和上边的式子，我们可以得出$E_{k+1}$中的边缘点与相应边缘线之间的几何关系，

类似地，结合平面特征点距离计算式和上边的式子，我们可以在$H_{k+1}$中的一个平面特征点和相应的平面块之间建立另一个几何关系，

最后，利用非线性优化的方法求解激光雷达的运动。对于$E_{k+1}$和$H_{k+1}$中的每个特征点$f_E$与$f_H$相加，我们得到一个非线性函数。

其中$f$的每一行对应一个特征点，$d$包含相应的距离。我们计算相对于$T_{k+1}^{\mathbf{L}}$的$f$的雅可比矩阵，记为$J$，其中$J=\partial f/\partial T_{k+1}^{\mathbf{L}}$。然后将$d$最小化为零，通过使用高斯-牛顿法（GN法）$$J^{T}JX=-J^{T}f$$迭代来求解上式。这里涉及到一个帧间运动的雅克比矩阵$J$，下面我们来推导一下。

4.1、帧间运动的雅克比J的推导

对于一帧点云数据，有第一个点$p_s$和最后一个点$p_e$,以及任意时刻的点$p_t$。
首先，将任意时刻$t$的点重新投影到同一时刻$t_s$得到点