激光SLAM入门笔记(四):前端配准 I

内容基于深蓝学院课件，如有错误，烦请斧正，不胜感激。
本篇是ICP及其相关变型的介绍。

1.ICP

属于point-to-point ICP，掌握思想。

1.1 目的

求解两坨点云之间的转换关系。

1.2 数学表述

• 给定两坨点云
  X = { x 1 , x 2 , . . . , x N x } P = { p 1 , p 2 , . . . , p N p } X=\{x_1,x_2,...,x_{N_x}\} \\ P=\{p_1,p_2,...,p_{N_p}\} X={x1​,x2​,...,xNx​​}P={p1​,p2​,...,pNp​​}
  其中$x_i$和$p_i$表示点云的坐标，$N_x$和$N_p$分别表示两坨点云中的点的数量。
• 求解旋转矩阵$R$和平移向量$t$，使得误差最小
  $$E(R,t)=\frac{1}{N}\sum _{i=1}^N\Vert x_i-R{p}_i-t{\Vert }^2$$
  其中$N$表示两坨点云匹配点对的数量。

1.3 求解方法

1.3.1 已知对应点

特点

• 不迭代

步骤

1. 分别求两坨点云的质心
   $$\begin{gathered} u_x=\frac{1}{N}\sum _{i=1}^N{x}_i \\ {u}_p=\frac{1}{N}\sum _{i=1}^N{p}_i \end{gathered}$$
   $u_x$是点云$X$的质心，$u_p$是点云$P$的质心。
2. 去中心化
   $$\begin{gathered} x_i^{\prime }=x_i-u_x \\ {p}_i^{\prime }=p_i-u_p \end{gathered}$$
3. 构建$W$矩阵
   $$W=\sum _{i=1}^N{x}_i^{\prime }{p}_i^{\prime T}$$
4. 对$W$进行SVD分解
   $$W=U\left[\begin{array}{ccc}{\sigma }_1& 0& 0\\ 0& {\sigma }_2& 0\\ 0& 0& {\sigma }_3\end{array}\right]V^T$$
5. 得到$R$和$t$
   $$\begin{gathered} R=V{U}^T \\ t=u_x-R{u}_p \end{gathered}$$

推导

1. 排除$t$，目标锁定在$R$上
   $$\begin{array}{rl}E(R,t)& =\frac{1}{N}\sum _{i=1}^N\parallel x_i-R{p}_i-t{\parallel }^2\\ & =\frac{1}{N}\sum _{i=1}^N\parallel x_i-R{p}_i-t-u_x+R{u}_p+u_x-R{u}_p{\parallel }^2\\ & =\frac{1}{N}\sum _{i=1}^N\parallel (x_i-u_x-R(p_i-u_p))+(u_x-R{u}_p-t){\parallel }^2\\ & =\frac{1}{N}\sum _{i=1}^N[\parallel x_i-u_x-R(p_i-u_p){\parallel }^2+\parallel u_x-R{u}_p-t{\parallel }^2\\ & +2(x_i-u_x-R(p_i-u_p){)}^T(u_x-R{u}_p-t)]\\ & =\frac{1}{N}\sum _{i=1}^N[\parallel x_i-u_x-R(p_i-u_p){\parallel }^2+\parallel u_x-R{u}_p-t{\parallel }^2]\\ & +\frac{1}{N}\sum _{i=1}^{N}2(x_i-u_x-R(p_i-u_p){)}^T(u_x-R{u}_p-t)\end{array}$$
   其中
   $$\begin{array}{r}\frac{1}{N}\sum _{i=1}^{N}2(x_i-u_x-R(p_i-u_p){)}^T(u_x-R{u}_p-t)\\ =2(u_x-R{u}_p-t)\frac{1}{N}\sum _{i=1}^N(x_i-u_x-R(p_i-u_p){)}^T\\ =2(u_x-R{u}_p-t)(\frac{1}{N}\sum _{i=1}^N(x_i-u_x-R(p_i-u_p)){)}^T\\ =2(u_x-R{u}_p-t)(\frac{1}{N}\sum _{i=1}^N{x}_i-u_x+R{u}_p-\frac{1}{N}\sum _{i=1}^{N}R{p}_i{)}^T=0\end{array}$$
   因此
   $$\begin{array}{rl}E(R,t)& =\frac{1}{N}\sum _{i=1}^N[\parallel x_i-u_x-R(p_i-u_p){\parallel }^2+\parallel u_x-R{u}_p-t{\parallel }^2]\\ & =E_1(R)+E_2(R,t)\end{array}$$
   即
   $$\begin{array}{rl}{E}_1(R)& =\frac{1}{N}\sum _{i=1}^N\parallel x_i-u_x-R(p_i-u_p){\parallel }^2\\ E_2(R,t)& =\parallel u_x-R{u}_p-t{\parallel }^2\end{array}$$
   由于对于任意$R$，总能得到$t=u_x-R{u}_p$，使得$E_2(R,t)$取得最小值0。
   因此
   $$\begin{array}{rl}\min E(R,t)& =\frac{1}{N}\sum _{i=1}^N\parallel x_i-R{p}_i-t{\parallel }^2\end{array}$$
   可转变为
   $$\begin{array}{rl}\min E_1(R,t)& =\frac{1}{N}\sum _{i=1}^N\parallel x_i-u_x-R(p_i-u_p){\parallel }^2\end{array}$$
2. 简化$\min E_1(R)$
   $$\begin{array}{rl}\min E_1(R)& =\frac{1}{N}\sum _{i=1}^N\parallel x_i-u_x-R(p_i-u_p){\parallel }^2\\ & =\frac{1}{N}\sum _{i=1}^N\parallel x_i^{\prime }-R{p}_i^{\prime }{\parallel }^2\\ & =\frac{1}{N}\sum _{i=1}^N(x_i^{\prime T}{x}_i^{\prime }+p_i^{\prime T}{R}^{T}R{p}_i^{\prime }-2x_i^{\prime T}R{p}_i^{\prime })\\ & =\frac{1}{N}\sum _{i=1}^N(x_i^{\prime T}{x}_i^{\prime }+p_i^{\prime T}{p}_i^{\prime }-2x_i^{\prime T}R{p}_i^{\prime })\\ & =\frac{1}{N}\sum _{i=1}^N-2x_i^{\prime T}R{p}_i^{\prime }+\frac{1}{N}\sum _{i=1}^N(x_i^{\prime T}{x}_i^{\prime }+p_i^{\prime T}{p}_i^{\prime })\end{array}$$
   由于$\frac{1}{N}{\sum }_{i=1}^N(x_i^{\prime T}{x}_i^{\prime }+p_i^{\prime T}{p}_i^{\prime })$与$R$无关，
   因此$\min E_1(R)$等价于$\max {\sum }_{i=1}^N{x}_i^{\prime T}R{p}_i^{\prime }$
3. 将$\max {\sum }_{i=1}^N{x}_i^{\prime T}R{p}_i^{\prime }$转换为求矩阵迹
   $$\begin{array}{rl}{x}_i^{\prime T}R{p}_i^{\prime }& =\left[\begin{array}{ccc}{x}_{i0}^{\prime }& x_{i1}^{\prime }& x_{i2}^{\prime }\end{array}\right]R{p}_i^{\prime }\\ & =\left[\begin{array}{ccc}{x}_{i0}^{\prime }& x_{i1}^{\prime }& x_{i2}^{\prime }\end{array}\right]\left[\begin{array}{c}{\vec{R}}_0\\ {\vec{R}}_1\\ {\vec{R}}_2\end{array}\right]p_i^{\prime }\\ & =x_{i0}^{\prime }{\vec{R}}_0{p}_i^{\prime }+x_{i1}^{\prime }{\vec{R}}_1{p}_i^{\prime }+x_{i2}^{\prime }{\vec{R}}_2{p}_i^{\prime }\\ & ={\vec{R}}_0{x}_{i0}^{\prime }{p}_i^{\prime }+{\vec{R}}_1{x}_{i1}^{\prime }{p}_i^{\prime }+{\vec{R}}_2{x}_{i2}^{\prime }{p}_i^{\prime }\end{array}$$
   $$\begin{array}{rl}R{p}_i^{\prime }{x}_{i^{\prime }}^T& =\left[\begin{array}{c}{\vec{R}}_0\\ {\vec{R}}_1\\ {\vec{R}}_2\end{array}\right]\left[\begin{array}{ccc}{x}_{i0}^{\prime }{p}_i^{\prime }& x_{i1}^{\prime }{p}_i^{\prime }& x_{i2}^{\prime }{p}_i^{\prime }\end{array}\right]\\ & =\left[\begin{array}{ccc}{\vec{R}}_0{x}_{i0}^{\prime }{p}_i^{\prime }& {\vec{R}}_0{x}_{i1}^{\prime }{p}_i^{\prime }& {\vec{R}}_0{x}_{i2}^{\prime }{p}_i^{\prime }\\ {\vec{R}}_1{x}_{i0}^{\prime }{p}_i^{\prime }& {\vec{R}}_1{x}_{i1}^{\prime }{p}_i^{\prime }& {\vec{R}}_1{x}_{i2}^{\prime }{p}_i^{\prime }\\ {\vec{R}}_2{x}_{i0}^{\prime }{p}_i^{\prime }& {\vec{R}}_2{x}_{i1}^{\prime }{p}_i^{\prime }& {\vec{R}}_2{x}_{i2}^{\prime }{p}_i^{\prime }\end{array}\right]\end{array}$$
   图示
   
   因此
   $$\begin{array}{rl}\max \sum _{i=1}^N{x_i^{\prime }}^{T}R{p}_i^{\prime }& =\max \sum _{i=1}^{N}Trace(R{p}_i^{\prime }{x_i^{\prime }}^T)\\ & =\max Trace(R\sum _{i=1}^N(p_i^{\prime }{x_i^{\prime }}^T))\\ & =\max Trace(RH)\end{array}$$
4. 利用矩阵迹的性质进行转换，并证明$R=V{U}^T$
   引理及证明：
   
   假设矩阵$A$为正定对称矩阵，则对于任意的正交矩阵$B$，都有
   $$Trace(A)\ge Trace(BA)$$
   由于$H$并非正定对称矩阵，因此要构造一个正定对称矩阵：
       (1).对$H$进行SVD分解，即$H=U\Sigma V^T$
       (2).构建正交矩阵$X$，令$X=V{U}^T$
       (3).$XH=V{U}^{T}U\Sigma V^T=V\Sigma V^T$为正定对称矩阵
   对于任意的正交矩阵$B$，根据上述性质，可得
   $$Trace(XH)\ge Trace(BXH)$$
   因为$B$为任意正交矩阵，因此$BX$可以取遍所有的正交矩阵，当然也包括需要求解的旋转矩阵$R$，因此
   $$Trace(RH)\le Trace(XH)$$
   当$R=X$时，等式成立。
5. 综上所述
   $$\begin{gathered} R=X=V{U}^T \\ t=u_x-R{u}_p \end{gathered}$$

1.3.2 未知对应点

特点

• 迭代

步骤

1. 构造匹配点对
   对$P$中的点在$X$中寻找对应点（欧氏距离最小），构造匹配点对，数量为$N$。
2. 计算$R$和$t$
   根据1.3.1的方法求解$R$和$t$。
   （$R$是$R_{X\leftarrow P}$,$t$是$t_{X\leftarrow P}$）
3. 对点云姿态进行变换，计算误差
   将$R$和$t$作用于点云$P$，于是$P$相对于$X$就有了一个新的姿态。
   误差的计算如1.2中$E(R,t)$所示。
4. 不断迭代，直至误差小于某一个值
   未达到迭代终止条件，则继续进行1,2,3步。

参考

[LIDAR-SLAM] Iterative Closest Point (ICP)简单实现
[0024] Iterative Closest Points（迭代最近点）

2.PL-ICP

2.1 示意图

2.2 基本思想

• 激光点是对实际环境中曲面的离散采样
• 最好的误差尺度为当前激光点到实际曲面的距离，因此关键的问题是如何恢复曲面
  PL-ICP的思想是用分段线性的方法来对实际曲面进行近似，用激光点到最近两点连线的距离来模拟激光点到实际曲面的距离。

2.3 数学描述

变量说明

$p_i$：点云集合$P$中，点$p_i$的三维坐标
$x_j^i$：$T_k{p}_i$在点$x_{j_1}^i$,$x_{j_2}^i$组成的直线上的投影点
$x_{j_1}^i$,$x_{j_2}^i$：点云集合$X$中，距离$T_k{p}_i$最近的两个点
$n_i^T$：点$x_{j_1}^i$,$x_{j_2}^i$组成的直线的法向量（单位法向量）

目标函数

PL-ICP的目标函数表示了点到直线的距离，近似表示了点到曲面的距离。
PL-ICP也可转为point-to-point ICP。
$$\underset{T_k}{\min }\sum _i\parallel n_i^T(x_{j_2}^i-T_k{p}_i){\parallel }^2⟺\underset{T_k}{\min }\sum _i\parallel x_j^i-T_k{p}_i{\parallel }^2$$
其中，$T_k=\left[\begin{array}{cc}{R}_k& t_k\\ 0& 1\end{array}\right]$，$T_k{p}_i=R_k{p}_i+t_k$

2.4 求解

已知量

• 当前激光帧$P$
• 参考激光帧$X$
• 初始位姿$T_0$

未知量

每次迭代计算后的两帧激光之间的相对位姿$T^{\ast }$

算法流程

1. 把当前帧的数据$p_i$根据初始位姿进行变换，得到$T_k{p}_i$
2. 对于当前帧中的点$T_k{p}_i$，在参考帧中找到最近的两个点$x_{j_1}^i$,$x_{j_2}^i$，并计算此两点间直线的法向量$n_i^T$和投影点$x_j^i$
3. 计算点$T_k{p}_i$与投影点$x_j^i$的距离，去除误差过大的点
4. 最小化误差函数$$\underset{T_k}{\min }\sum _i\parallel x_j^i-T_k{p}_i{\parallel }^2$$

2.5 与ICP的区别

• 误差函数形式不同
  ICP的误差是点到点的误差，PL-ICP的误差实际是点到直线的误差（虽然也能转成点到点的误差）。
  PL-ICP的误差形式更符合实际情况。
• 误差收敛速度不同
  ICP为一阶收敛，误差是递减的
  $$\parallel T_k-T_{\infty }\parallel <k\parallel T_{k-1}-T_{\infty }\parallel$$
  PL-ICP为二阶收敛，误差的平方和是递减的
  $$\parallel T_k-T_{\infty }{\parallel }^2<k\parallel T_{k-1}-T_{\infty }{\parallel }^2$$
• PL-ICP的求解精度高于ICP，特别是在结构化环境中
• PL-ICP对初始值更敏感，因此不单独使用，而是与里程计、CSM等一起使用
  PL-ICP对初始值敏感，容易陷入局部极值。
  CSM不容易陷入局部极值（为何？）。

3.NICP

3.1 示意图

3.2 基本思想

• 较ICP而言，在对应点匹配方面纳入了法向量和曲率
  利用法向量和曲率对错误的匹配点进行滤除。
• 误差项除了考虑对应点的欧氏距离之外，还考虑对应点法向量的角度差

3.3 数学描述

符号

$p_i,p_j$：点云集合$P^C,P^R$中点的三维坐标
$n_i$：点$i$处曲面的法向量
${\sigma }_i$：点$i$处曲面的曲率
$T$：欧氏变换矩阵
$\zeta$：两个点云集合$P^C,P^R$中对应点对的集合
$\widetilde{p_i}=\left[\begin{array}{c}{p}_i\\ n_i\end{array}\right]$
$T⨁\widetilde{p_i}=\left[\begin{array}{c}R{p}_i+t\\ R{n}_i\end{array}\right]$

误差

$e_{ij}(T)=({\tilde{p}}_i^c-T⨁{\tilde{p}}_j^r)$
(前三维，欧氏距离的误差；后三维，法向量的误差)

目标函数

定义为
$$\sum _{\zeta }{e}_{ij}(T{)}^T{\tilde{\Omega }}_{ij}{e}_{ij}(T)$$
而
$${\tilde{\Omega }}_{ij}=\left[\begin{array}{cc}{\Omega }_i^s& 0\\ 0& {\Omega }_i^n\end{array}\right]$$
${\tilde{\Omega }}_{ij}$为信息矩阵。

⚠️法向量和曲率的计算

• 找到点$p_i$周围半径$R$球形空间内的所有点$V_i$
  $$\begin{gathered} u_i^s=\frac{1}{|V_i|}\sum _{p_j\in V_i}{p}_i \\ {\sum }_i^s=\frac{1}{|V_i|}\sum _{p_j\in V_i}(p_i-u_i{)}^T(p_i-u_i) \\ {\sum }_i^s=R\left[\begin{array}{cc}{\lambda }_1& 0\\ 0& {\lambda }_2\end{array}\right]R^T \end{gathered}$$
• 曲率
  $$\begin{gathered} {\sigma }_i=\frac{{\lambda }_1}{{\lambda }_1+{\lambda }_2} \\ ({\lambda }_2\gg {\lambda }_1) \end{gathered}$$
• 法向量
  最小特征值对应的特征向量（为何？）
• 信息矩阵
  $$\begin{gathered} {\Omega }_i^s=({\sum }_i^s{)}^{-1} \\ {\Omega }_i^n=\{\begin{array}{l}R\left[\begin{array}{cc}\frac{1}{\epsilon }& 0\\ 0& 1\end{array}\right]R^T\text{曲率足够小}\\ I\end{array} \end{gathered}$$

3.4 点匹配规则

• 如果没有well define的法向量，则拒绝
• 两点间的欧式距离大于阈值，则拒绝
  $$\parallel p_i^c-T⨁p_j^r\parallel >{ϵ}_d$$
• 两点的曲率之差大于阈值，则拒绝
  $$|\log {\sigma }_i^c-\log {\sigma }_j^r|>{ϵ}_{\sigma }$$
• 两点的法向量角度之差大于阈值，则拒绝
  $$n_i^c\cdot T⨁n_j^r<{ϵ}_n$$

3.5 求解

如前所述，目标函数定义为
$$\sum _{\zeta }{e}_{ij}(T{)}^T{\tilde{\Omega }}_{ij}{e}_{ij}(T)$$
该非线性最小二乘问题使用LM方法进行求解

3.6 特点

• 在寻找点匹配过程中，考虑了环境曲面的法向量和曲率，因此可以提前排除一些明显是错误的匹配
• 在误差定义中，除了考虑欧式距离之外，还考虑了法向量之间的距离，因此具有更加准确的角度（激光里程计的误差主要来源于角度）
• 在开源领域，效果最好的ICP匹配方法

3.7 算法流程总结

• 计算参考激光帧和当前激光帧中每一个点的法向量和曲率
• 根据当前解，把当前激光帧的点转换到参考坐标系中，并且根据欧氏距离、法向量、曲率等信息来选择匹配点（也有可能没有匹配点）
• 根据上面介绍的方法，用LM方法进行迭代求解，迭代收敛即可得到两帧激光数据之间的相对位姿

4.IMLS-ICP

4.1 示意图

4.2 基本思想

拟合出一个曲面，求当前点到曲面的距离，但并没有显式的表达式来表达拟合过程。

• 选择具有代表性的激光点来进行匹配，既能减少计算量同时又能减少激光点分布不均匀导致的计算结果出现偏移
• 点云中隐藏着真实的曲面，最好的做法就是能从参考帧点云中把曲面重建出来
• 曲面重建的越准确，对真实世界描述越准确，匹配的精度就越高

4.3 代表点的选取

• 具有丰富特征的点，即为结构化的点–具有良好的曲率和法向量的定义
• 曲率越小的点越好，因为曲率为0代表着直线，代表着最结构化的点，也代表着具有非常 好的法向量定义，能够提供足够的约束
• 选点的时候需要注意选取的激光点的均衡以保证可观性，因为是平面匹配，不存在角度不可观的情况。只需要考虑X方向和Y方向的可观性。要保证两者的约束基本上是一致的，才能让结果不出现偏移

4.4 曲面重建

$P_k$：前$n$帧激光数据组成的子图
$n_i$：点云集合$P_k$中的点$p_i$的法向量
$I^{P_k}(x)$：${\mathbb{R}}^3$空间的点$x$到点云集合$P_k$隐藏曲面的距离
$I^{P_k}(x)$定义如下
$$I^{P_k}(x)=\frac{{\sum }_{p_i\in P_k}{W}_i(x)((x-p_i)\cdot {\vec{n}}_i)}{{\sum }_{p_i\in P_k}{W}_j(x)}$$
（实际上完成了曲面拟合，基于假设、隐式拟合得到隐藏曲面）
其中，权重$W_i(x)$定义如下
$$W_i(x)=e^{-\parallel x-p_i{\parallel }^2/h^2}$$
（$I^{P_k}(x)$=0表示点云集合$P_k$隐藏的曲面）

4.5 匹配求解

$S_k$：新一帧激光数据
$I^{P_k}(x_i)$：当前帧$S_k$中的点$x_i$到曲面的距离
${\vec{n}}_i$：$P_k$中距离点$x_i$最近的点的法向量

• 点$x_i$在曲面上的投影$y_i$为：
  $$\begin{gathered} x_i^{\prime }=R{x}_i+t \\ {y}_i=x_i^{\prime }-I^{P_k}(x_i^{\prime }){\vec{n}}_i \end{gathered}$$
• 对于新一帧激光数据$S_k$，通过最小化以下函数，求解姿态变换：
  $$\sum _{x_i\in S_k}((R{x}_i+t-y_i)\cdot {\vec{n}}_i{)}^2$$