点云配准及运动估计算法

点云配准及运动估计算法

虽然激光slam的前端配准，主流思路是基于交叉迭代优化算法，即分为两步优化，优化点云配准与优化运动估计。但是关于点云匹配的评价函数有不同的思路。评价函数用于评价运动估计(R,t)的可靠性。就是估计的(R,t)对不对。

根据匹配点关系，建立目标评价函数函数，把激光的帧间匹配的运动估计问题转换为求解目标函数的极值问题。目前个人见过两种：

1. 基于距离判断：

思路：将两帧点云数据在同一个坐标系下，一帧数据中的点找到另一帧数据最近的点，就作为一对匹配点。

代表算法：ICP类算法：ICP/PLICP/NICP/IMLS等
评价方程：多种，点到点(ICP)/点到线(PLICP)/点到面的距离等，详细解释查看ICP相关介绍
优化目标：最小化评价方程，得到R，t

• 点到点距离作为评价方程(ICP)：
  ICP算法介绍
  匹配点在同一坐标下的范数(距离平方)的大小，即评价匹配点的距离。
  
  点到点距离作为约束方程，容易卡在局部最优解。
  

• NICP:
  NICP算法介绍
  加入特征点所在曲面法向量的约束，去掉一些错误匹配的点。
  匹配评价方程仍使用距离匹配。

• 点到线的距离作为评价方程(PLICP)：
  paper: 《An ICP variant using a point-to-line metric》
  paper中介绍的point-to-point约束方程：
  
  $S^{ref}$表示点所在曲面
  Π { S r e f , p i } \Pi\{S^{ref}, p_i\} Π{Sref,pi​}表示激光点$p_i$在曲面上的投影

  为什么使用投影点与原始点距离作为约束，因为激光在相邻两帧之间有激光打到同一点云的概率很小，因此在ICP算法中，上述的约束方程是实际比较常用的匹配算法。而不是在两帧点云中找到匹配点。

  paper中提出的约束方程：
  
  分析，相比之前的方程多乘了一个曲面法向量，因此将会提高逼近速度。
  
  ICP中的得到的激光点连线来近似曲面，上图(a)，(b)为文中说的曲面匹配的点到点算法。©为本文的约束实际实现方法，找到上一帧与目标点最近的两个相邻点，计算这两点与目标点的距离。
  带w表示世界坐标系下的点的坐标描述。以下为算法的实现步骤，感觉论文在(4)那里没有描述好

• 点到面的评价方法:Point-to-Plane ICP
  
  评价方程：点到(对应点所在)面的距离
  $M_{opt}={\mathrm{a}\mathrm{r}\mathrm{g}\mathrm{m}\mathrm{i}\mathrm{n}}_M{\sum }_i{((M\bullet s_i-d_i)\bullet n_i)}^2$
  $s_i$指第I个source point
  $d_i$指第i个destination point
  $M$为变换矩阵

• IMLS:
  paper：《IMLS-SLAM: scan-to-model matching based on 3D data》
  计算当前扫描中一点$x_i$到曲面的距离$I^{P_k}(x_i)$，曲面是前n次扫描计算得到的环境曲面。$P_k$为当前得到的所有点集。
  则当前帧的评价方程：
  
  由于$I^{P_k}(x_i)$含有指数，因此不好迭代下降优化，因此用以下方程近似点到面的距离：
  
  其中$n_j$为曲面上$(x_i)$最近点的曲面法向量。
  $y_j$为$(x_i)$在曲面上的投影。

通过评价方程可以看出，IMLS是一种点到面的约束方法，评价函数上与Point-to-Plane的差异也比较小：
Point-to-Plane：目标对应点的法向量*原始点与原始点在平面上的投影点的连线
IMLS：目标对应点的法向量*目标点与原始点连线

2. 基于概率似然模型判断：

papers：
《Real-Time Correlative Scan Matching》
《The Normal Distributions Transform_A New Approach to Laser Scan Matching》
主要是针对栅格地图的slam算法，计算匹配点落在同一个栅格中的概率。

目前见过两种评价函数方法：
思路1：网格使用高斯概率模型，实现连续可优化
代表算法：NDT，CSM等
评价函数1：详细介绍查看NDT相关介绍
解释：评价两帧点云的分布情况

$x_i=(x,y{)}^T$表示激光点坐标

$q_i,{\Sigma }_i$表示点$x_i$对应的高斯分布的均值和方差。

Score函数：

优化目标1：

思路2：计算目标匹配点转换后占用地图的概率，用‘1-‘变成最小化函数。
评价函数2：
解释：匹配点的地图占用概率

其中：
表示$S_i(T)$激光点i用T转换之后的坐标

$M(x)$表示得到的坐标x的地图占用概率，可利用双线性插值得到cell的概率拟合，即使用插值法可以拟合得到参考点云的概率分布函数$M(x)$。

优化目标2：最小化目标评价函数

参考链接：
https://blog.csdn.net/weixin_43236944/article/details/88188532