最邻近点迭代（Iterative Closest Point，ICP）算法，通过李群、李代数推导、并实现

还在修改，谨慎参考　

 

该篇文章是通过李群和李代数左扰动模型求取雅可比矩阵，然后通过非线性最小二乘方法对点云配准迭代ICP算法进行的一个推导，这是这篇文章与SVD求解详解的不同之处。由于作者认识和理解有限，文章存在众多不足之处，同时请谅解作者不严谨的态度，请大家不吝指教！

　　最近在做点云配准的项目，需要用到考虑法线因素的点云配准，我把实现流程贴出来与大家交流一下。

　　如果您不太了解李代数的左扰动模型，推荐：https://www.cnblogs.com/gaoxiang12/p/5577912.html，也可参考《视觉SLAM十四讲》。

　　作者大致说一下，自己对该过程的认识。首先ICP采用了迭代求解策略（迭代求解一般应用于非线性问题求解中）。首先在迭代计算之初，会提供一个初始值：$T_0$($T_0$可设为单位阵$I$)，我们需要计算一个增量$\Delta T$，以此设下一步迭代值为$T_1=T_0\oplus \Delta T $（本着较为严谨的表达，我将增量与迭代值的运算符号使用了$\oplus$来表示。在不同的模型里面，$\oplus$代表了不同的运算规则。通常，迭代计算中$\oplus$符号会表示加法，而在此文章中表示左乘）。计算$\Delta T$是一个非线性求解的过程，在本文中所用的非线性求解方法是Gauss-Newton方法（可见公式（）），在该表达式中，$\Delta T$是所求的增量，$J$为雅可比矩阵，$f(x)$为误差表达式。而在Gauss-Newton方法中的$J$，就是我们需要用李群和李代数来求取的。

　　废话不多说。

一、非线性方程求解方法

　　求解非线性最小二乘问题一般采用迭代求解方法，ICP算法就是一个迭代求解的过程。即，点云从一个起始的状态（用$T_k$表示），经过计算产生了$\Delta T$下一步状态值$T_{k+1}=T_k\oplus\Delta T$；再将$T_{k+1}$作为下一次计算的起始值，计算出再下一步的增量值$\Delta T$计算出状态值$T_{k+2}$。以此计算点云状态值$T$直至收敛到一个确定的值。这样就是ICP算法迭代的过程。

　　在本文中采用了Gauss-Newton方法来求解出每步迭代的增量$\Delta T$，（Gauss-Newton方法是一种较为常用的非线性最小二乘求解方法。如果你不太了解非线性最小二乘方法，我依旧推荐书籍《视觉SLAM十四讲》或者查阅其余文档、博客）。

　　在本文直接给出了Gauss-Newton的表达式，如公式（）。

$$ \begin{equation}\begin{split} J(x){J^T(x)}\Delta x=-J(x)f(x) \end{split}\end{equation} $$

　　在公式中$(n)$中，$J(x)$为雅可比矩阵，$J(x)J^T(x)$为牛顿方法中二阶Hessian矩阵的近似。$f(x)$为误差表达式，$\Delta x$即为我们所需要计算的增量

　　该方程组是关于变量$\Delta x$的线性方程组，将$J(x)J^T(x)$定义为$H$,$-J(x)f(x)$定义为$g$,那么式$()$可以表示为:

$$ \begin{equation}\begin{split} H\Delta x=g \end{split}\end{equation} $$

　　在我看来这个迭代过程的核心问题就是找到$J$矩阵 ，然后顺利求取$\Delta x$的值，并将$x_k\oplus\Delta x$作为下次迭代的值。

注意：我将寻找$J$作为Gauss-Newton迭代方法的核心问题，是因为在确定了目标方程的前提下，公式$()$中，只有$J$是未知量。如果$J$已知（或者已知获取方式）那么这个迭代求解的问题就迎刃而解了。在本文中，标题中所谓的“李群和李代数推导”就是指用其推导ICP问题的$J$表达式（作者以此理解不知道是否有误，还请各位多指教）。

二、李群、李代数

在本节作者希望你能了解变换矩阵T是一种李群$SE(3)$，它对应的李代数是$\xi $；旋转矩阵对应李群$SO(3)$，对应李代数为$\phi $。

在本节开始，为了更加侧重ICP算法的推导过程而不是李群和李代数的，所以作者只是给出来$SE(3)$和$SO(3)$的扰动模型。强烈推荐各位详细参考高博士的博客（文初已经给出）。

       $SO(3)$扰动模型（左乘）：

$$ \begin{equation}\begin{split} \frac{\partial(Rp)}{\partial{\varphi}}={\lim\limits_{\varphi \to0}}{\frac{exp(\varphi^{\wedge})exp(\phi^{\wedge} )p-exp(\phi^{\wedge})p}{\varphi }} \end{split}\end{equation} $$

　　该式求导为：

$$ \begin{equation}\begin{split} \frac{\partial(Rp)}{\partial{\varphi}}={-(Rp)}^{\wedge} \end{split}\end{equation} $$

　　但这个公式只是推导了旋转矩阵用李代数求导的过程，点云配准中大部分是需要考虑到点云的旋转和平移的，所以接下来是对变换矩阵进行李代数求导的推导。

　　$SE(3)$扰动模型推导：

　　$T$左乘上一个扰动$\Delta{T}=exp({{\delta\xi}})$，设扰动项的李代数为$\Delta{T}=exp({{\delta\xi}})$

$$ \begin{equation}\begin{split} \frac{\partial(Tp)}{\partial{\delta\xi}}={\lim\limits_{\delta\xi \to0}}{\frac{exp(\delta\xi^{\wedge})exp(\xi^{\wedge} )p-exp(\xi^{\wedge})p\; }{\delta\xi}} \end{split}\end{equation} $$

　　最终对$SE(3)$的李代数推导结果为：

$$ \begin{equation}\begin{split} \frac{\partial(Tp)}{\partial{\delta\xi}}=\begin{bmatrix}
　　I & -(Rp+t)^{\wedge}\\
　　0^{T} &0 ^{T}
　　\end{bmatrix} \end{split}\end{equation} $$

　　上式矩阵是一个$4*6$的矩阵，这矩阵在该ICP推导中作为迭代算法中的雅可比矩阵。结合ICP算法的目标方程，我们来看一下扰动模型是怎么求取该目标方程的$J$j矩阵的。

　　ICP算法的目标方程为：

$$ \begin{equation}\begin{split} F(x)={\frac{1}{2}}{\sum_{i=1}^{n}}{\begin{Vmatrix} Tq_i-p_i \end{Vmatrix}}^2 \end{split}\end{equation} $$

$$ \begin{equation}\begin{split} f(x)=Tq_i-p_i \end{split}\end{equation} $$

$$ \begin{equation}\begin{split} J(x)=\frac{\partial{f(x)}}{\partial\xi} \end{split}\end{equation} $$

即：

$$ \begin{equation}\begin{split} J(x)=\frac{\partial{(Tq_i-p_i)}}{\partial\xi} \end{split}\end{equation} $$

三、ICP算法推导

　　ICP算法的目标方程为：

$$ \begin{equation}\begin{split} F(x)={\frac{1}{2}}{\sum_{i=1}^{n}}{\begin{Vmatrix} Tq_i-p_i \end{Vmatrix}}^2 \end{split}\end{equation} $$

　　令$f(x)=Tq_i-p_i$，将$f(x)$对$T$求导过程转变为$f(x)$对$T$的李代数$\phi$求导的过程，如公式$(n)$

　　即：

$$ \begin{equation}\begin{split} \frac{\partial{f(x)}}{\partial{T}}=\frac{\partial{f(x)}}{\partial{\xi}}=\begin{bmatrix}　　I & -(Rp+t)^{\wedge}\\ 　　0^{T} &0 ^{T}　　\end{bmatrix} \end{split}\end{equation} $$

　　该矩阵即作为迭代求解中的$J$雅可比矩阵；

四、总结：

　　对前面所讲解的东西进行一个串联，究竟上面说的那些东西到底是什么关系。

　　首先我们应该知道ICP算法是一个迭代求解的过程，我们需要不断的去求解增量更新求解的变换参数，所谓增量就是我们上述所说的$\Delta x$，经过$x_{k+1}={x_k}+\Delta x$这么一个过程，让最终的$x$逼近我们想要的那个值。那么也许会有同学会问那迭代初始值，即$x_0$怎么来的，在ICP中这个初始值可以选择单位矩阵。

　　我们上述的Gauss-Newton方法和LM方法，它们就是用来计算所谓的增量$\Delta x$的，但这里的增量$\Delta x$并不会以$x_{k+1}={x_k}+\Delta x$的形式去更新迭代值，而是将$\Delta x$转换为$SE(3)$形式去左乘迭代值$T_k$，以此得到下一个迭代值$T_{k+1}$。我认为这与$J$的求取是有关的，但作者自己也存在一些疑惑所以不能断言，请读者理解我们需要通过Gauss-Newton方法去求解$\Delta x$以此更新迭代值。

　　我们知道迭代过程中需要求解增量$\Delta x$，并且求增量用的是Gauss-Newton方法。可以注意到在Gauss-Newton方法公式$(n)$中，有三个符号分别是$J，f(x)，\Delta x$，在ICP中$f(x)$为$Tq_i-p_i$，$\Delta x$是我们需要求解的值，最后剩下$J$,$J$就是我们需要由扰动模型(公式$（）$)求解的那个雅可比矩阵。

五、实现

　　好了，我们说了这么多前提，那我就将我所用的推导过程在这跟大家展示出来，但这里只列出数学表达式，如果你想知道Matlab具体的实现过程，请到我的Github上下载我所实现的代码。

　　第一：列目标方程为：

　　$$ \begin{equation}\begin{split} F(x)={\frac{1}{2}}{\sum_{i=1}^{n}}{\begin{Vmatrix} Tq_i-p_i \end{Vmatrix}}^2 \end{split}\end{equation} $$

　　使用$SE(3)$扰动模型对$f({x_i}）={Tp_i}-{q_i}$进行求导，得到雅可比矩阵$J_i$

　　$$ \begin{equation}\begin{split} J_i=\begin{bmatrix}
　　I & -(Rq_i+t)^{\wedge}\\
　　0^{T} &0 ^{T}
　　\end{bmatrix} \end{split}\end{equation} $$

　　我们已知Gauss-Newton方法的表达式为公式$()$，这里：

$$ \begin{equation}\begin{split} J(x){J^T(x)}\Delta x=-J(x)f(x) \end{split}\end{equation} $$

$g$表示如下：

$$ \begin{equation}\begin{split} g={\sum\limits_{i=1}^{n}J_i^{T}f(x)_i} \end{split}\end{equation} $$

　　最后使用公式$()$求的增量$\Delta \xi$，$\xi$表示$SE(3)$的李代数。

$$ \begin{equation}\begin{split} H{\Delta \xi}=g \end{split}\end{equation} $$

将$\Delta \xi$转换为$SE(3)$形式，$\xi=\begin{bmatrix} \rho \\ \phi \end{bmatrix}$，$\phi $表示的是旋转矩阵的李代数，令$\theta = norm(\phi)$即$\phi$的模，$a=\frac{\phi}{\left \| \phi\right \|}$

$$ \begin{equation}\begin{split} exp(\xi^\wedge)=\begin{bmatrix} exp(\phi ^\wedge) &J\rho \\ 0^T &1 \end{bmatrix} \end{split}\end{equation} $$

$$ \begin{equation}\begin{split} exp(\phi ^\wedge)=exp(\theta a^\wedge)=cos\theta I+(1-cos\theta )aa^T+sin{\theta}a^\wedge \end{split}\end{equation} $$

$$ \begin{equation}\begin{split} J=\frac{sin\theta}{\theta}I+(1-\frac{sin\theta}{\theta})aa^T+\frac{1-cos\theta}{\theta}a^\wedge \end{split}\end{equation} $$

　　求得增量以后对迭代值进行更新，并重复上述过程，设置迭代结束条件，直到迭代终止你就能看到迭代结果。

　　那么，该ICP过程推导已经结束，最后附上该代码的网址：https://github.com/Hany-trio/ICP-Iterative-Closest-Point

　　作为新人请大家多多包涵，不吝指教