激光SLAM

帧间匹配方法：

• Point-to-Plane ICP
• NDT
• Feature-based Method

回环检测方法：

• Scan-to-Scan
• Scan-to-Map

LOAM创新点

• 定位和建图的分离：
  • 里程计模块：高频低质量的帧间运动估计
  • 建图模块：低频高质量的用于点云的精细匹配和配准
• 基于特征的帧间匹配
• 提取边缘特征点(edge)和平面特征点(plannar)

激光雷达里程计

特征点提取

第k次扫描(sweep)的点云表示为${\mathcal{P}}_k$，其中一个扫描点$i\in {\mathcal{P}}_k$，令${\mathcal{S}}_i$表示同一扫描中点i周围的点集，点i的曲率：
$$c_i=\frac{1}{|\mathcal{S}|\Vert X_{\left(k,i\right)}^L\Vert }\Vert \sum _{j\in \mathcal{S},j\ne i}\left(X_{\left(k,i\right)}^L-X_{\left(k,j\right)}^L\right)\Vert (1)$$
根据曲率值对一次扫描中的点进行排序，选取曲率最大的点为边缘特征点(edge point)，选取曲率最小的点为平面特征点(plannar point)。边缘特征点的曲率需要大于某个阈值，平面特征点的曲率需要小于某个阈值。已经被选择的特征点附近的点不再考虑，选择的特征点总量也有限制。
另外，还需要排除一些异常值：
1）排除平行于激光扫描平面的点，候选点及其附近点集合拟合的平面与激光线束的夹角不能过小，对应(a)；
2）排除遮挡区域边界的点，候选点附近的点集中没有与候选点在激光束方向上间距过大的点，对应(b)。

特征匹配

基于特征的Scan-to-Scan匹配方法
在开始于$t_k$的第k次sweep扫描完成之后，${\mathcal{P}}_k$被重投影到时刻$t_{k+1}$，记作${\bar{\mathcal{P}}}_k$，并存储在3D KD-tree中用于快速检索，以方便之后的匹配。

在第k+1次sweep扫描开始后，${\mathcal{P}}_{k+1}$随着接收到更多的点而逐渐增长，按照上一章的提取方法提取边缘特征点和平面特征点。令${\mathcal{E}}_{k+1}$和${\mathcal{H}}_{k+1}$分别表示k+1次sweep边缘特征点和平面特征点的集合。从${\bar{\mathcal{P}}}_k$中找到边缘线作为${\mathcal{E}}_{k+1}$中点的对应，平面块作为${\mathcal{H}}_{k+1}$中点的对应。

激光里程计在一次扫描过程中迭代估计自身运动，在每次迭代中，使用当前估计的变换将将${\mathcal{E}}_{k+1}$和${\mathcal{H}}_{k+1}$重投影到扫描开始时刻，记为${\tilde{\mathcal{E}}}_{k+1}$和${\tilde{\mathcal{H}}}_{k+1}$。

下图左侧表示寻找边缘点的对应边缘线的过程，设i为${\tilde{\mathcal{E}}}_{k+1}$中的一个点，$i\in {\tilde{\mathcal{E}}}_{k+1}$，边缘线由两个点表示。设j是i在${\bar{\mathcal{P}}}_k$中的最近邻点，l是i在j的相邻线束扫描中的最近邻点。(j,l)构成了i的对应边缘线。然后需要根据曲率式(1)验证j和l都是边缘点。这样做的考虑是单个扫描线不能包含来自同一边缘线的多个点，只有一个例外：边缘线平行于扫描平面，这种情况是异常情况不会提取特征点。
下图右侧表示寻找平面点的对应平面块，与边缘点类似，在${\bar{\mathcal{P}}}_k$中找i的最近邻点j。然后在j的相同扫描线和相邻扫描线中分别找i的最近邻点l和m，这保证了三个点不共线。然后根据曲率公式验证是平面点。

点到线的距离：
$$d_{\mathcal{E}}=\frac{\left|\left({\tilde{\mathbf{X}}}_{(k+1,i)}^L-{\bar{\mathbf{X}}}_{(k,j)}^L\right)\times \left({\tilde{\mathbf{X}}}_{(k+1,i)}^L-{\bar{\mathbf{X}}}_{(k,l)}^L\right)\right|}{\left|{\tilde{\mathbf{X}}}_{(k+1,i)}^L-{\bar{\mathbf{X}}}_{(k,l)}^L\right|}$$
点到面的距离：
$$d_{\mathcal{H}}=\frac{\left|\left({\tilde{\mathbf{X}}}_{(k+1,i)}^L-{\bar{\mathbf{X}}}_{(k,j)}^L\right)\cdot \left({\bar{\mathbf{X}}}_{(k,j)}^L-{\bar{\mathbf{X}}}_{(k,l)}^L\right)\times \left({\bar{\mathbf{X}}}_{(k,j)}^L-{\bar{\mathbf{X}}}_{(k,m)}^L\right)\right|}{\left|\left({\bar{\mathbf{X}}}_{(k,j)}^L-{\bar{\mathbf{X}}}_{(k,l)}^L\right)\times \left({\bar{\mathbf{X}}}_{(k,j)}^L-{\bar{\mathbf{X}}}_{(k,m)}^L\right)\right|}$$
令${\mathbf{T}}_{k+1}^L$代表$t_{k+1}$到$t$的雷达位姿变换
点-线残差：
$$f_{\mathcal{E}}({\mathbf{X}}_{(k+1,i)}^L,{\mathbf{T}}_{k+1}^L)=d_{\mathcal{E}},i\in {\mathcal{E}}_{k+1}$$

点-面残差：
$$f_{\mathcal{H}}({\mathbf{X}}_{(k+1,i)}^L,{\mathbf{T}}_{k+1}^L)=d_{\mathcal{H}},i\in {\mathcal{H}}_{k+1}$$
将所有点-线残差和点-面残差结合起来得到残差矢量
$$\mathbf{f}({\mathbf{T}}_{k+1}^L)=\mathbf{d}$$
最小二乘问题：
$$\underset{{\mathbf{T}}_{k+1}^L}{\min }F(x)=\frac{1}{2}{\Vert \mathbf{f}({\mathbf{T}}_{k+1}^L)\Vert }_{\Omega }^2$$
其中${\Vert \mathbf{x}\Vert }_{\Omega }^2={\mathbf{x}}^T\Omega \mathbf{x}$，$\Omega$是权重矩阵，又叫信息矩阵。
该算法为每个特征点分配一个二方权重。与其对应的距离较大的特征点被分配较小的权重，距离大于阈值的特征点被认为是异常值并分配零权重。