对点云匹配算法ICP、PL-ICP、NICP和IMLS-ICP的理解

点云匹配算法是为了匹配两帧点云数据，从而得到传感器（激光雷达或摄像头）前后的位姿差，即里程数据。匹配算法已经从最初的ICP方法发展出了多种改进的算法。他们分别从配准点的寻找，误差方程等等方面进行了优化。下面分别介绍：

ICP

ICP的基本思想是：
给定两个点云集合

X = { x 1 , x 2 , ⋯   , x N x } X=\left\{x_{1}, x_{2}, \cdots, x_{N_{x}}\right\} X={x1​,x2​,⋯,xNx​​}

$P=\left\{p_1,p_2,\cdots ,p_{N_p}\right\}$

其中 $x_i$和$p_i$表示点云坐标，$N_x$和$N_p$表示点云的数量。
求解旋转矩阵R和平移向量t使得下式结果最小。

$E(R,t)=\frac{1}{N_p}{\sum }_{i=1}^{N_p}{\Vert x_i-R{p}_i-t\Vert }^2$

在实际工程中不可能知道两个点云的点是如何配对的。只能通过迭代求解的方法一步步缩小误差，最终得到使误差方程最小的旋转矩阵R和平移矩阵t。

算法流程：
1）寻找对应点
通常使用编码盘的里程计数据得到位姿差，即当前机器人在上次机器人坐标系中的位姿。将此R和t作为ICP算法的first guess，帮助算法寻找点云对应点。这里需注意，如果激光传感器没有安装在机器人坐标系中心，则存在里程计得到的位姿到激光传感器位姿的坐标转换关系。如下图中$l$所示：

2）根据对应点计算R和t。
这一步就是根据找好的对应点构建误差方程。普通的ICP 是使用点到点的距离作为误差的。误差方程如下：

$E(R,t)=\frac{1}{N_p}{\sum }_{i=1}^{N_p}{\Vert x_i-R{p}_i-t\Vert }^2$

然后求解出R和t。具体的求解推导如下：

$\begin{array}{rl}E(R,t)& =\frac{1}{N_p}\sum _{i=1}^{N_p}{\Vert x_i-R{p}_i-t\Vert }^2\\ E(R,t)& =\frac{1}{N_p}\sum _{i=1}^{N_p}{\Vert x_i-R{p}_i-t-u_x+R{u}_p+u_x-R{u}_p\Vert }^2\\ =& \frac{1}{N_p}\sum _{i=1}^{N_p}{\Vert x_i-u_x-R\left(p_i-u_p\right)+\left(u_x-R{u}_p-t\right)\Vert }^2\\ =& \frac{1}{N_p}\sum _{i=1}^{N_p}{\Vert x_i-u_x-R\left(p_i-u_p\right)\Vert }^2+{\Vert u_x-R{u}_p-t\Vert }^2\\ & +2{\left(x_i-u_x-R\left(p_i-u_p\right)\right)}^T\left(u_x-R{u}_p-t\right)\\ & =\frac{1}{N_p}\sum _{i=1}^{N_p}{\Vert x_i-u_x-R\left(p_i-u_p\right)\Vert }^2+{\Vert u_x-R{u}_p-t\Vert }^2\end{array}$

其中

$u_x=\frac{1}{N_p}{\sum }_{i=1}^{N_p}{x}_i{u}_p=\frac{1}{N_p}{\sum }_{i=1}^{N_p}{p}_i$

而$\left(x_i-u_x-R\left(p_i-u_p\right)\right)$项累加$N_p$次后会等于0。

最终得到的等式中只有${\Vert x_i-u_x-R\left(p_i-u_p\right)\Vert }^2$项与$R$有关。

可以先令${\Vert u_x-R{u}_p-t\Vert }^2$等于0，则只需保证${\Vert x_i-u_x-R\left(p_i-u_p\right)\Vert }^2$项最小即可。求出$R$后带入${\Vert u_x-R{u}_p-t\Vert }^2=0$求解出t。所以误差方程可以简化成下式：

$\min E(R,t)=\frac{1}{N_p}{\sum }_{i=1}^{N_p}{\Vert x_i-u_x-R\left(p_i-u_p\right)\Vert }^2$

$=\frac{1}{N_p}{\sum }_{i=1}^{N_p}{\Vert x_i^{\prime }-R{p}_i^{\prime }\Vert }^2=\frac{1}{N_p}{\sum }_{i=1}^{N_p}{x}_i^T{x}_i^{\prime }+p_i^T{R}^{T}R{p}_i^{\prime }-2x_i^{T}R{p}_i^{\prime }$

$={\sum }_{i=1}^{N_p}-2x_i^{\prime T}R{p}_i^{\prime }$

因为$x_i^T{x}_i^{\prime }$项与$R$矩阵没有关系，所以可以不考虑。由于$R$为正交矩阵，则$R^{T}R=I$，进而$p_i^T{R}^{T}R{p}_i^{\prime }=p_i^T{p}_i^{\prime }$。可以看到该项与$R$也没有什么关系，同样不考虑它。最后就只剩下一项与$R$相关了。

要求最小即最大化下面的式子：

$\max {\sum }_{i=1}^{N_p}{x}_i^{\prime T}R{p}_i^{\prime }={\sum }_{i=1}^{N_p}\mathrm{Trace}\left(R{x}_i^{\prime }{p}_i^T\right)=\mathrm{Trace}(RH)$

其中$H={\sum }_{i=1}^{N_p}{x}_i^{\prime }{p}_i^{\prime T}$

利用定理，假设矩阵A为正定对称矩阵，则对于任意的正交矩阵B，都有

$\mathrm{Trace}(A)\ge \mathrm{Trace}(BA)$

对$H$进行SVD分解$H=U\Lambda V^T$

令$X=V{U}^T$，则$X$为正交矩阵。

$XH=V{U}^{T}U\Lambda V^T=V\Lambda V^T$得到正定对称矩阵。

则：

$\mathrm{Tr}\mathrm{ace}(XH)\ge \mathrm{Tr}\mathrm{ace}(BXH)$

B是任意的正交矩阵，X也是一个正交矩阵，因此BX可以取遍所有的正交矩阵。所以BX也包括了需要求解的旋转矩阵T，因此

$\mathrm{Trace}(RH)\le \mathrm{Trace}(XH)$

当R=X时，等式成立。因此：

$R=X=V{U}^T$

则$\mathrm{t}=u_x-R{u}_p$

3）将计算得到的R和t用于下一次迭代计算，直到误差值小于设定的阈值。

这里指出ICP的一个明显缺陷：
两帧激光点云数据中的点不可能表示的是空间中相同的位置。所以用点到点的距离作为误差方程势必会引入随机误差。

参考论文：Least-Squares Fitting of Two 3-D Point Sets

PL-ICP

PL-ICP相对于PP-ICP最大的区别是其改进了误差方程。PP-ICP是点对点的距离作为误差而PL-ICP是采用点到其最近两个点连线的距离。下图展示了误差方程的差异。

从上方的(a)中可以看出，激光点是对实际环境中曲面的离散采样。最好的误差尺度是激光点到实际曲面的距离。而PL-ICP采用分段线性的方法对实际曲面进行近似，用激光点到最近两点连线的距离来模拟实际激光点到曲面的距离。可以看出PL-ICP的误差方程更贴近实际情况。

算法流程：
先贴一张论文里的算法步骤

1）给定一个初始的转换矩阵$q_0$，将当前激光帧的数据转换到参考帧坐标系下。初始的转换矩阵$q_0$一般通过里程计来获得。后面迭代计算所需的$q_k$由上一次算法迭代计算得到。
2）为当前激光帧中的每一个点，找到其最近的两个点j1和j2。
3）去除误差过大的点。
4）构建最小化误差方程。
5）求解出位姿转换矩阵${\mathbf{q}}_{k+1}$。然后将其用于下次迭代计算。

总结PL-ICP与ICP的主要区别：
1）误差函数的形式不同，ICP对点对点的距离作为误差，PL-ICP为点到线的距离作为误差；
PL-ICP的误差形式更符合实际情况。
2）收敛速度不同，ICP为一阶收敛，PL-ICP为二阶收敛。
$\Vert {\mathbf{q}}_k-{\mathbf{q}}_{\infty }\Vert <c\Vert {\mathbf{q}}_{k-1}-{\mathbf{q}}_{\infty }\Vert {\Vert {\mathbf{q}}_k-{\mathbf{q}}_{\infty }\Vert }^2<c{\Vert {\mathbf{q}}_{k-1}-{\mathbf{q}}_{\infty }\Vert }^2$
3）PL-ICP的求解精度高于ICP，特别是在结构化环境中。
4）PL-ICP对初始值更敏感。不单独使用。其容易陷入局部循环，如下图(a)所示。通常用里程计得到一个初始转换矩阵$q_0$给到PL-ICP算法。

csm源码包分析：
ros的csm包实现了ICP和PL-ICP算法。源码包和论文下载地址如下：
https://censi.science/software/csm/
csm包的作者给出了一个该功能包的操作说明文件（csm_manual.pdf）。里面详细描述了各项配置参数的含义。
其中sm/app文件夹中的sm0.c sm1.c sm2.c sm3.c 相当于是几个使用示例。
主要的算法实现是在csm/icp文件夹中的几个文件里。论文中的所有算法步骤完整的体现在了icp_loop.c文件中的icp_loop函数里。
开源代码
参考论文：An ICP variant using a point-to-line metric

NICP

NICP方法与ICP方法的主要流程和思想是一致的。但是它在Trim outlier和误差项里考虑了更多的因素。这也是它效果更好的原因。它充分利用实际曲面的特征来对错误点进行了滤除，主要使用的特征为法向量和曲率。在误差项里除了考虑了点到对应点切面的距离，还考虑了对应点法向量的角度差。目前NICP方法开源的代码主要是针对3D点云的，其调用了Eigen库和OpenCV库。源码中显示的部分调用了QT5。既然NICP方法考虑了法向量和曲率，那么就涉及到了如何求解点的法向量和曲率。下面简述论文中的方法：
1）高斯拟合。找到点$p_i$周围半径$R$范围内的所有点$V_i$。求解均值和协方差。
$\begin{array}{rl}{\mu }_i^s& =\frac{1}{\left|{\mathcal{V}}_i\right|}\sum _{{\mathbf{p}}_j\in {\mathcal{V}}_i}{\mathbf{p}}_i\\ {\mathbf{\Sigma }}_i^s& =\frac{1}{\left|{\mathcal{V}}_i\right|}\sum _{{\mathbf{p}}_j\in {\mathcal{V}}_i}{\left({\mathbf{p}}_i-{\mu }_i\right)}^T\left({\mathbf{p}}_i-{\mu }_i\right)\end{array}$

2）对协方差矩阵进行SVD分解，得到按从小到大顺序的特征值${\lambda }_{1:3}$。
${\mathbf{\Sigma }}_i^{\mathrm{s}}=\mathbf{R}\left(\begin{array}{ccc}{\lambda }_1& 0& 0\\ 0& {\lambda }_2& 0\\ 0& 0& {\lambda }_3\end{array}\right){\mathbf{R}}^T$

论文中曲率的定义为：
${\sigma }_i=\frac{{\lambda }_1}{{\lambda }_1+{\lambda }_2}$
法向量的定义：最小特征值对应的特征向量。

Trim outlier中的改进：

1）如果没有well define的法向量，则拒绝。即选择比较结构化的点，如果对应点周围过于杂乱就丢弃该点。
2）两点间的距离大于阈值，则拒绝。
$\Vert {\mathbf{p}}_i^{\mathrm{c}}-\mathbf{T}\oplus {\mathbf{p}}_j^{\mathrm{r}}\Vert >{ϵ}_d$
3）两点的曲率之差距大于阈值，则拒绝。
$\left|\log {\sigma }_i^{\mathrm{c}}-\log {\sigma }_j^{\mathrm{r}}\right|>{ϵ}_{\sigma }$
4）两点的法向量角度之差大于阈值，则拒绝。
${\mathbf{n}}_i^{\mathrm{c}}\cdot \mathbf{T}\oplus {\mathbf{n}}_j^{\mathrm{r}}<{ϵ}_n$

误差项中的改进：

1）点到对应点切面的距离作为其中的一个误差项。
2）法向量的夹角作为另一个误差项。考虑该项误差增加了旋转矩阵$R$的求解精度。

最后得到的误差定义为：

${\mathbf{e}}_{ij}(\mathbf{T})=\left({\tilde{\mathbf{p}}}_i^{\mathrm{c}}-\mathbf{T}\oplus {\tilde{\mathbf{p}}}_j^{\mathrm{r}}\right)$

其中
$T\oplus {\tilde{\mathrm{p}}}_i=\left(\begin{array}{c}R{p}_i+t\\ R{n}_i\end{array}\right)$
即同时考虑了欧式距离和法向量的夹角。

目标函数的求解：
1）目标函数的定义为：

${\sum }_c{\mathbf{e}}_{ij}(\mathbf{T}{)}^T{\tilde{\Omega }}_{ij}{\mathbf{e}}_{ij}(\mathbf{T})$

2）非线性最小二乘问题，通过LM方法进行求解。

$\begin{array}{rl}(\mathbf{H}+\lambda \mathbf{I})\Delta \mathbf{T}& =\mathbf{b}\\ H& =\sum _{J_i{J}_i}^T\\ J_i& =\frac{\partial e_{ij}(T)}{\partial T}\mathbf{T}\leftarrow \Delta \mathbf{T}\oplus \mathbf{T}\end{array}$

对NICP的总结：
1）由于在寻找点匹配的过程中，考虑了环境 曲面的法向量和曲率，因此可以提前排除 一些明显是错误的匹配。这样就减少了计算量并且提高了计算结果的精度。
2）在误差定义中，除了考虑欧式距离之外，还考虑了法向量之间的夹角，因此具有更加准确的求解角度。
3）用LM方法进行迭代求解目标误差方程，迭代收敛即可得到两帧激光数据之间的相对位姿。
源码地址
参考网址：http://jacoposerafin.com/nicp/

IMLS-ICP

IMLS-SLAM是一个仅依赖点云数据的低漂移（low-drift）SLAM算法。其依赖于一个scan-to-model的匹配框架。这里的model可以认为是对点云进行的局部曲面建模。
基本思想：
1）选择具有代表性的激光点来进行匹配，既能减少计算量同时又能减少激光点分布不均匀导致的计算结果出现偏移。
2）点云中隐藏着真实的曲面，最好的做法是能从参考帧点云中把曲面重建出来。
3）曲面重建的越准确，对真实世界描述越准确，匹配的精度就越高。
具体的改进措施：
1）scan egomotion
egomotion的定义如下：

进行激光数据的运动畸变去除。在雷达扫描一圈的过程中，车子是在运动的。这会造成一帧激光的时间内，每束激光测量时车子实际的位置是不同的。论文里假设两次连续的激光扫描间隔，车子的egomotion是相似的。所以采用上一次的相对位移来计算当前的实际的egomotion。
2）去除动态障碍物
在匹配scan和model之前，我们要去除scan中所有移动物体。我们采用去除所有可能会动的小物体的方法：首先，删去地面点云，聚类，并抛弃所有（bounding box的）长小于14m，宽小于14m，高小于4m的物体。剩下的结构足够我们匹配。最后添上去掉的地面点云。
3）特征点的选取
选取思路：

• 具有丰富特征的点，即为结构化的点：具有良好的曲率和法向量的定义。

• 曲率越小的点越好，因为曲率为0代表着直线，代表着最结构化的点，也代表着具有非常好的法向量定义，能够提供足够的约束。

• 选点的时候需要注意选取的激光点的均衡以保证可观性，因为是平面匹配，不存在角度不可观的情况。只需要考虑X方向和Y方向的可观性。要保证两者的约束基本上是一致的， 才能让结果不出现偏移。

• 大概率上，随着角度的偏转，观测的点云是不一样的。所以角度一般是可观的。对于二维SLAM，只需保证X方向和Y方向上选取的点云数量接近就可以。而对于三维则需要考虑6个维度（X,Y,Z,roll,pitch,yaw）。

4）曲面重建

• 已知点云集合$P_k$中每一个点$p_i$的法向量$n_i$ ,则$P_k$中隐藏的曲面为：
  $\begin{array}{rl}{I}^{P_k}(x)& =\frac{{\sum }_{p_i\in P_k}{W}_i(x)\left(\left(x-p_i\right)\cdot \overrightarrow{n_i}\right)}{{\sum }_{p_j\in P_k}{W}_j(x)}\\ W_i(x)& =e^{-{\Vert x-p_i\Vert }^2/h^2}\end{array}$

$x$是空间中的一个点，$pi$是点云中的一个点。

上图中的$Height=(x-p_i)\cdot n_i$，表示点到曲面的距离。
我们认为激光点云是分布在真实曲面的附近，并可以用高斯分布描述。如下图

所以可以用$W_i(x)$表示点$x$到点云$p_i$距离的权重。当点$x$到点云$p_i$距离很远时，权重会接近0。该算法会选取点$x$附近的一部分点使用上面的公式重建曲面。公式中的${\sum }_{p_j\in P_k}{W}_j(x)$为所有这些权重的累加。

公式的实现代码

    double mh2 = m_h * m_h;
    for(int i = 0; i < nearPoints.size(); ++i)
    {
        Eigen::Vector2d delta_p = x - nearPoints[i];
        double weight = std::exp(-delta_p.squaredNorm()/mh2);
        projSum += weight * delta_p.dot(nearNormals[i]);
        weightSum += weight;
    }
    height = projSum / (weightSum + 0.000001);//加一个很小的数避免除0

• $I^{P_{\mathrm{k}}}(x)=0$对应的集合表示曲面。
• 空间中点到曲面的距离，直接代入上述方程即可。

5)匹配求解

• 当前帧中一点$x_i$ 到曲面的距离为$I^{P_k}(x_i)$

• 则点$x_i$在曲面上的投影$y_i$为：${\mathrm{y}}_i=x_i-I^{P_k}\left(x_i\right)\cdot n_i$
  $P_k$中离点$x_i$最近的点的法向量为$n_i$。利用这个公式寻找$x_i$对应的投影点$y_i$。

• 点$x_i$和点$y_i$为对应的匹配点：$\sum {\left(\left(\mathrm{R}{\mathrm{x}}_{\mathrm{i}}+\mathrm{t}-{\mathrm{y}}_{\mathrm{i}}\right)\cdot {\mathrm{n}}_{\mathrm{i}}\right)}^2$
  这里的法向量$n_i$是$y_i$点的法向量。该公式描述的是转换后的$x_i$点到投影点$y_i$上的距离（注意是法向量上的距离）。

总结：
IMLS-ICP使用高斯拟合和最小二乘重建出一个隐含的曲面。找到空间点在隐含曲面的投影点。使用点到该曲面上投影点间的距离构建误差方程。

参考网址：
IMLS-SLAM 论文学习
IMLS-SLAM: scan-to-model matching based on 3D data,Jean-Emmanuel Deschaud

相关论文放到我的github里了：
相关论文

ICP方法初始值的设定方法

一个好的初始值可以减少icp迭代的次数，提高效率和效果。初始值的设定可以从下面三个角度来考虑：
1.、假设机器人静止，设置帧间位移为0。
2、假设机器人匀速运动，利用上一时刻机器人的速度和时间，计算当前时刻机器人位移间隔，将此作为初值传入icp
3、利用传感器如里程计、IMU等，计算前后两时刻的初始位移。实际使用时要注意坐标系的转换。因为icp方法实在激光坐标系下进行的。

提升ICP方法精度和速度的一些考虑

精度上：
1）使用其他传感器或方法（如里程计、IMU）提供初值（coarse to fine）。
2）Ransac框架，去除outlier。
效率上：
1）选取合适的采样点。IMLS-ICP的论文里就提到了一些去除误差点的方法。
2）使用合适的数据结构，提高程序的运行效率。

关注公众号《首飞》回复“机器人”获取精心推荐的C/C++，Python，Docker，Qt，ROS1/2，机器人学等机器人行业常用技术资料。