迭代最近邻ICP算法

概念

有关ICP问题中求解旋转矩阵和平移向量的知识在论文“Least-Squares Fitting of Two 3-D Point Sets”中有详细介绍。
ICP算法的思路就是：找到两组点云集合中距离最近的点对，根据估计的变换关系（$R,t$）来计算距离最近点对经过变换之后的误差，进过不断的迭代直至误差小于某一阈值或者达到迭代次数来确定最终的变换关系。

1. ICP算法能够使两个不同坐标系下的点集匹配到一个坐标系中，这个过程就是配准，配准的操作就是找到从坐标系1变换到坐标系2的刚性变换；
2. ICP的本质就是配准，但有不同的配准方案，ICP算法本质是基于最小二乘的最优配准方法。该方法重复进行选择对应关系对，计算最优刚体变换，直到满足正确配准的收敛精度要求；
3. ICP算法的目的就是找到待匹配点云数据与参考点云数据之间的旋转参数 $R$ 和平移参数 $t$,使得两点数据之间满足某种度量准则下的最优匹配；
4. 每次操作的都是目标点集，使目标点集不断靠近参考点集，因此求 $R$ 和 $t$ 也是每次考虑目标点集中每个点在参考点集中的最近点。

功能

通过给定的俩组空间点云坐标信息来解算俩个坐标系之间的位姿变换信息（$R,t$）。

步骤

两个点集$P_1,P_2$，每一步迭代，都朝着距离最小的目标进行。

1. 筛选点对：由$P_1$中的点，在$P_2$中搜索出其最近的点，组成一个点对；找出两个点集中所有的点对。点对集合相当于进行有效计算的两个新点集。
2. 跟据点集对，即两个新点集，计算两个质心。
3. 由新点集，计算出下一步计算的旋转矩阵$R$，和平移矩阵$t$(其实来源于质心的差异)。
4. 得到旋转矩阵和平移矩阵$R,t$，就可以计算点集$P_1$进行刚体变换之后的新点集$P_2$，由计算$P_1$到$P_2$的距离平方和，以连续两次距离平方和之差绝对值，作为是否收敛的依据。若小于阈值，就收敛，停止迭代。
5. 重复1-5，直到收敛或达到既定的迭代次数。
   其中，计算旋转矩阵$R$和平移向量$t$时，需要矩阵方面的运算。

下面主要针对已知精确的空间点云集匹配关系的条件下，求解旋转矩阵$R$和评议向量$t$的过程。
已有精准匹配的点云集$p_i,p_i^{\prime }（i=1\cdots n）$，俩者之间存在关系等式$$p_i^{\prime }=R{p}_i+t+N_i$$其中$R$是旋转矩阵，$t$是平移向量，$N_i$是对应的噪声（为了简洁我们默认没有噪声）。
为了求解精确的旋转矩阵$R$和平移向量$t$，最小化投影误差平方和$$\sum _{i=1}^n||p_i^{\prime }-(R{p}_i+t)|{|}^2$$获取来个点集的质心$$\begin{gathered} p=\frac{1}{n}\sum _{i=1}^n{p}_i \\ {p}^{\prime }=\frac{1}{n}\sum _{i=1}^n{p}_i^{\prime } \end{gathered}$$进行去质心处理$$\begin{gathered} q_i=p_i-p \\ {q}_i^{\prime }=p_i^{\prime }-p^{\prime } \end{gathered}$$于是最小化投影误差信息$$\begin{gathered} \sum _{i=1}^n||p_i^{\prime }-(R{p}_i+t)|{|}^2 \\ =\sum _{i=1}^n||p_i^{\prime }-R{p}_i-t-p+R{p}^{\prime }+p-R{p}^{\prime }|{|}^2 \\ =\sum _{i=1}^n||(p_i^{\prime }-p^{\prime }-R(p_i-p))+(p^{\prime }-Rp-t)|{|}^2 \\ =\sum _{i=1}^n||p_i^{\prime }-p^{\prime }-R(p_i-p)|{|}^2+||p^{\prime }-Rp-t|{|}^2+2(p_i^{\prime }-p^{\prime }-R(p_i-p){)}^T(p^{\prime }-Rp-t) \end{gathered}$$值得注意的是$(p_i^{\prime }-p^{\prime }-R(p_i-p))$部分在求和之后为0，因此上述等式为$$\sum _{i=1}^n||p_i^{\prime }-p^{\prime }-R(p_i-p)|{|}^2+||p^{\prime }-Rp-t|{|}^2$$最小化该等式右侧项可以用来获取平移向量$$t=p^{\prime }-Rp$$通过最小化$$\begin{gathered} \sum _{i=1}^n||p_i^{\prime }-p^{\prime }-R(p_i-p)|{|}^2 \\ =\sum _{i=1}^n||q^{\prime }-Rq|{|}^2 \end{gathered}$$来求解旋转矩阵信息$R$ $$\begin{gathered} \sum _{i=1}^n||q^{\prime }-Rq|{|}^2 \\ =\sum _{i=1}^n(q_i^{\prime T}{q}_i^{\prime }+q_i^T{R}^{T}R{q}_i-2q^{\prime T}R{q}_i) \end{gathered}$$使$$\sum _{i=1}^n{q}_i^{\prime T}R{q}_i$$最大。
假设$$\begin{gathered} W=\sum _{i=1}^n{q}_i^{\prime }{q}_i^T \\ ⟹W=U\Sigma V^T \end{gathered}$$为了能使得${\sum }_{i=1}^n{q}_i^{\prime T}R{q}_i$最大，$R=U{V}^T$使得$$MAX\sum _{i=1}^n{q}_i^{\prime T}R{q}_i=U\Sigma U^T$$有关这一部分的证明在论文“Least-Squares Fitting of Two 3-D Point Sets”中$(11)-(16)$有详细说明。
至此，我们获得了旋转矩阵$R=U{V}^T$的求解公式以及平移向量$t=p^{\prime }-Rp$的求解公式。

链接

ICP算法

补充