通常，使用RGB-D相机或是其他方法获取到物体的三维点云后，由于采集设备不同、拍摄视角不同等等因素的影响，即使是同一个物体所得到的点云也会有较大的差异，主要是旋转或者平移的变化。对于一组图像数据集中的两幅图像，需要通过寻找一种空间变换把一幅图像映射到另一幅图像，使得两图中对应于空间同一位置的点一一对应起来，从而达到信息融合的目的。所以，就需要对点云进行配准。
迭代最近点算法（ICP）是一种点云匹配算法。其思想是：通过旋转、平移使得两个点集之间的距离最小。ICP算法由Besl等人于1992年提出，文献可以参考：A Method for Registration of 3D Shapes，另外还可以参考：Least-Squares Fitting of Two 3-D Point Sets。前者使用的是四元数方法来求解旋转矩阵，而后者则是通过对协方差矩阵求解SVD来得到最终的旋转矩阵。流程大体上一致，后面就主要列出前者的流程。

ICP

我们可以使用四元数表示旋转关系： $\vec{q_{R}} = {[\begin{matrix} q_{0} & q_{1} & q_{2} & q_{3} \end{matrix}]}^{T}$ ，满足条件： $q_{0} \geq 0$ 、 $q_{0}^{2} + q_{1}^{2} + q_{2}^{2} + q_{3}^{2} = 1$ 。
由于点的坐标是用 ${[\begin{matrix} x & y & z \end{matrix}]}^{T}$ 表示，如果要对其做某个旋转或者平移变换，无法也用四元数来表示，所以要通过下面的公式得到四元数所表示的旋转矩阵：

\begin{matrix} (1) & R_{\vec{q_{R}}} = [\begin{matrix} q_{0}^{2} + q_{1}^{2} q_{2}^{2} q_{3}^{2} & 2 (q_{1} q_{2} q_{2} q_{3}) & 2 (q_{1} q_{3} + q_{0} q_{2}) \\ 2 (q_{1} q_{2} + q_{2} q_{3}) & q_{0}^{2} q_{1}^{2} + q_{2}^{2} q_{3}^{2} & 2 (q_{2} q_{3} q_{0} q_{1}) \\ 2 (q_{1} q_{3} q_{0} q_{2} & 2 (q_{2} q_{3} + q_{0} q_{1}) & q_{0}^{2} q_{1}^{2} q_{2}^{2} + q_{3}^{2} \end{matrix}] \end{matrix}

注：虽然四元数相对难理解一些，但是稍加对比就能发现，使用四元数来表示旋转关系能减少很多参数。迭代时可是要多次用到这个旋转关系的，可以节约不少计算量。这样，我们只需要在最后通过上式将四元数表示的旋转关系转换回旋转矩阵即可。
平移向量可以表示为：

\begin{matrix} (2) & T = {[\begin{matrix} q_{4} & q_{5} & q_{6} \end{matrix}]}^{T} \end{matrix}

1、假设待配准的点集为： $P = {\vec{p_{i}}}$ ；模板点集为： $X = {\vec{x_{i}}}$ 。
接下来要从带配准的点集 $P$ 中提取出与模板点集 $X$ 中的每个点最近的匹配点，度量方式采用欧式距离。对于模板点集 $X$ 中的每个点，求其到到点集 $P$ 中每个点的欧氏距离，取出欧氏距离最小的那个点，作为对应点放入新的对应点集 $P^{^{'}}$ 。为表述简洁，第2节开始后面提到的点集 $P$

标签

迭代计算

四元数

poi

迭代最近点（Iterative Closest Point, ICP）算法及matlab实现

ICP