【激光SLAM】激光的前端配准算法-编程知识

文章目录

ICP匹配方法（Point to Point）
PL-ICP匹配方法（Point to Line）
基于优化的匹配方法（Optimization-based Method）
- 优化方法的求解
- 地图双线性插值
- - 拉格朗日插值法——一维线性插值
相关方法（Correlation-based Method）
- 帧间匹配似然场
- - 算法流程
  - 位姿搜索
- 分枝定界算法
引用

在激光SLAM中，前端配准（Frontend Registration）是实现定位和地图构建的关键步骤之一。它的作用是将当前帧的激光扫描数据与先前帧（或参考帧）的激光扫描数据进行配准，以获取它们之间的相对位姿变换。
通过前端配准，激光SLAM系统可以实现对机器人在环境中的位置和姿态的估计，以及构建准确的地图。前端配准的准确性对于后续的位姿图优化、环路闭合和地图更新等步骤具有重要影响，因此它是激光SLAM中的关键组成部分。

前端配准在视觉内叫做Tracking或者帧间匹配，对激光SLAM是有非常大的影响的
帧间匹配不一定说的是前后两帧进行匹配，也可以是任意帧之间进行匹配
是一个Map—>Scan的过程，一个Scan和一个Map去匹配的过程

ICP匹配方法（Point to Point）

基本目的： 两个点云进行配准
在这里插入图片描述
数学描述：

设x_i和p_i是匹配的，即两个点云中的点打向的是物理空间中的同一个点。因此，我们需要找到R和t，使得两个匹配点的距离最小：

在这里插入图片描述

已知对应点的求解方法（解析解）：

u_x表示点云集合X的几何中心（基准位姿/平均位姿），u_p表示点云集合P的几何中心（基准位姿/平均位姿），随后把每个点云都移动到平均位姿来。
在这里插入图片描述
对W进行SVD分解：

则ICP的解为：

在这里插入图片描述

效果如下：

在这里插入图片描述
已知对应点的求解方法——证明：

在这里插入图片描述

未知对应点的求解方法：

在这里插入图片描述

PL-ICP匹配方法（Point to Line）

PL-ICP 实际就是point-to-line ICP,是点到线的ICP方法。相比于传统的ICP方法,即点到点的匹配方法，PL-ICP的流程其实和ICP的流程是一样的，不同的地方在于ICP是找最近邻的一点，以点与点之间的距离作为误差，而PL-ICP是找到最近邻的两点，两点连线，是以点到线的距离作为误差。

ICP的匹配算法的缺点：

在这里插入图片描述

假设棕色那条线是实际场景中的墙，蓝色是t时刻的扫描点，红色的点是t+1时刻的扫描点，如果按照ICP的思想，t+1时刻的1点会匹配到t时刻的靠上的点（因为欧式距离更短），但是1点的正确匹配点应该是靠下的点，所以这个匹配就是错误的，所引起的额外的匹配误差是由于ICP本身的算法引起的。因为匹配算法强制的认为他们是空间中的同一个点，但是对于激光雷达，前后两帧的激光数据很有可能是打到不同的物体或者说是同一物体的不同部位（类似ICP在点云配准中，前后是匹配的），所以这是ICP算法的缺点。

PL-ICP算法的原理：

室内环境通常是结构化环境，即譬如墙壁等有众多规则的曲面，而激光数据实际是对实际环境中曲面的离散采样，因此，最好的误差尺度就是点到实际曲面的距离。我们目前通过传感器已经知晓激光点云数据，实际的重点就是对环境中曲面的恢复。PL-ICP 采用分段线性的方式近似替代曲面，从而获得激光点到实际曲面的距离。

所以最好的方法就是计算离最近邻两点组成的直线的距离，如下图：

在这里插入图片描述

数学描述：

在这里插入图片描述

如（2）式，是ICP算法的目标函数数学表达式；（3）式PLICP算法的目标函数表达式，比ICP多了一个直线的法向量。

在这里插入图片描述可以看做是t时刻的参考帧，看做是k+1时刻经过R和t变化到t时刻的当前帧，这两个之间的差值是一个向量。那么ICP的算法求的就是误差向量1的模的平方，而PL-ICP算法就是秋误差向量1在法向量n_i上的投影（也就是到直线的距离）。目标函数就是求所有这些距离和的最小值，从而得到R和t。