PCL中的点云配准（Registration）ICP算法

引言

给定两个来自不同坐标系的三维数据点集，找到两个点集空间的变换关系，使得两个点集能统一到同一坐标系统中，即配准过程。配准的目标是在全局坐标框架中找到单独获取的视图的相对位置和方向，使得它们之间的相交区域完全重叠。对于从不同视图（views）获取的每一组点云数据，需要一个能够将它们对齐在一起的单一点云模型，从而可以应用后续处理步骤，如分割和对象重构。
这篇博文就基于PCL中的ICP算法，实现不同视图点云数据的配准，并延伸相关知识。

配准模型

两个(pairwise)点云数据集的配准，即在其中一个数据上进行旋转和平移，使得他们之间的相交区域完全重合。这里输出结果是一个（4x4）刚性（rigid）变换矩阵。

两个数据集的计算步骤：
1. 识别最能代表两个数据集中的场景的兴趣点（interest points）（即关键点 keypoints）
2. 在每个关键点处，计算特征描述符;
3. 从特征描述符集合以及它们在两个数据集中的 $x,y,z$ 位置，基于特征和位置之间的相似性来估计对应关系;
4. 假设数据被认为包含噪声的，并不是所有的对应关系都是有效的，所以舍弃对配准过程产生负面影响的那些负影响对应关系;
5. 从剩下的一组好的对应关系中，估计一个变换行为。

关键点

点云中的特征点是一些兴趣点，如目标的转点、角点等具有明显特征（feature）的点或点集。

特征描述

基于发现的关键点，提取特征组合信息并生成向量来进行比较。

一致性估计 correspondence estimate

给定两组数据获取相应的特征描述向量，找到相应的特征以在数据中找到重叠的部分。

舍弃糟糕的一致性估计

不是所有的一致性估计都是正确的，错误的一致性将对变换产生负影响，因此需要舍弃。

变换 transformation

步骤：
1. 根据一致性评价误差度量（error metric）
2. 估计变换矩阵
3. 优化点结构

迭代最近点 Iterative Closest Point （ICP）

ICP算法本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对，计算最优刚体变换这一过程，直到满足正确配准的收敛精度要求。ICP是一个广泛使用的配准算法，主要目的就是找到旋转和平移参数，将两个不同坐标系下的点云，以其中一个点云坐标系为全局坐标系，另一个点云经过旋转和平移后两组点云重合部分完全重叠。ICP算法最早由Chen and Medioni，and Besl and McKay提出。

算法的输入：参考点云和目标点云，停止迭代的标准。
算法的输出：旋转和平移矩阵，即转换矩阵。

基本过程

对于目标点云中的每个点，匹配参考点云（或选定集合）中的最近点。
求得使上述对应点对计算均方根（root mean square，RMS）最小的刚体变换，求得平移参数和旋转参数。
使用获得的转换矩阵来转换目标点云。
迭代（重新关联点），直到满足终止迭代的条件（迭代次数或误差小于阈值）。这里的误差最小，可以是相邻两次均方根差的绝对值小于某一限差。

可能存在的问题

算法收敛于局部最小误差。
噪声或异常数据可能导致算法无法收敛或错误。
在进行ICP算法第一步要确定一个迭代初值，选取的初值将对最后配准结果产生重要的影响，如果初值选择不合适，算法可能就会限入局部最优，使得迭代不能收敛到正确的配准结果。

刚性（rigid）变换矩阵

旋转矩阵是欧式空间中执行旋转变换的矩阵，ICP算法在做点云配准时，变换矩阵包含了平移和旋转，因此，这里有必要研究一下坐标系的平移和旋转过程。三维变换与二维变换相似，采用齐次坐标技术来描述空间的各点坐标及其变换，描述空间三维变换的变换矩阵是4×4的形式。由此，一系列变换可以用单个矩阵来表示。

以坐标系的三个坐标轴 $X,Y,Z$ 分别作为旋转轴，则点实际上只在垂直坐标轴的平面上作二维旋转。此时用二维旋转公式就可以直接推出三维旋转变换矩阵。规定在右手坐标系中，物体旋转的正方向是右手螺旋方向，即从该轴正半轴向原点看是逆时针方向。

两角和差公式：

sin (α + β) = sin α \cdot cos β + cos α \cdot sin β sin (α - β) = sin α \cdot cos β - cos α \cdot sin β cos (α + β) = cos α \cdot cos β - sin α \cdot sin β cos (α - β) = cos α \cdot cos β + sin α \cdot sin β

$\sin (\alpha +\beta)=\sin \alpha \cdot \cos \beta + \cos \alpha \cdot \sin \beta\\ \sin (\alpha -\beta)=\sin \alpha \cdot \cos \beta - \cos \alpha \cdot \sin \beta\\ \cos (\alpha +\beta)=\cos \alpha \cdot \cos \beta - \sin \alpha \cdot \sin \beta\\ \cos (\alpha -\beta)=\cos \alpha \cdot \cos \beta + \sin \alpha \cdot \sin \beta\\$
二维坐标旋转，推导过程：
绕坐标原点，以半径

r $r$ 旋转一定的角度

α $\alpha$ ，如下图中，点

(x,y) $(x,y)$ 逆时针经过旋转

α $\alpha$ 变换得到

(x′,y′) $(x',y')$ ，已知旋转角度

α $\alpha$ 和点

(x,y) $(x,y)$ 坐标，计算出点

(x′,y′) $(x',y')$

x' = r \cdot cos (α + β) = r \cdot cos α \cdot cos β - r \cdot sin α \cdot sin β = x \cdot cos α - y \cdot sin α y' = r \cdot sin (α + β) = r \cdot sin α \cdot cos β + r \cdot cos α \cdot sin β = x \cdot sin α + y \cdot cos α [x' y'] = [cos α sin α - sin α cos α] [x y]

$x'=r\cdot \cos (\alpha +\beta)=r \cdot \cos \alpha \cdot \cos \beta - r\cdot \sin \alpha \cdot \sin \beta=x\cdot \cos \alpha- y\cdot \sin \alpha\\y'=r\cdot \sin (\alpha +\beta)=r \cdot \sin \alpha \cdot \cos \beta +r\cdot \cos \alpha \cdot \sin \beta=x\cdot \sin \alpha+ y\cdot \cos \alpha\\ \begin{bmatrix} x' \\ y' \end{bmatrix} = \begin{bmatrix} \cos \alpha & -\sin \alpha\\\sin \alpha &\cos \alpha \end{bmatrix} \begin{bmatrix} x\\y \end{bmatrix}$

下面直接给出右手系中绕三轴的基本旋转矩阵，选装角度分别位 $\theta,\alpha,\beta$ :

R x (θ) = ⎡ ⎣ ⎢ 100 0 cos θ sin θ 0 - sin θ cos θ ⎤ ⎦ ⎥

$R_x(\theta)=\begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta\\0 & \sin\theta & \cos\theta \end{bmatrix}$

R y (α) = ⎡ ⎣ ⎢ cos α 0 - sin α 010 sin α 0 cos α ⎤ ⎦ ⎥

$R_y(\alpha)=\begin{bmatrix} \cos\alpha& 0 & \sin\alpha \\ 0 & 1 & 0\\-\sin\alpha & 0 & \cos\alpha \end{bmatrix}$

R z (β) = ⎡ ⎣ ⎢ cos β sin β 0 - sin β cos β 0 001 ⎤ ⎦ ⎥

$R_z(\beta)=\begin{bmatrix} \cos\beta& -\sin\beta & 0 \\ \sin\beta & \cos\beta & 0 \\0 & 0 & 1 \end{bmatrix}$
通用旋转矩阵

R $R$ 为：

R = R z (β) R y (α) R x (θ)

$R=R_z(\beta)R_y(\alpha)R_x(\theta)$
这里的旋转矩阵旋转过程，先绕

X $X$ 轴旋转

θ $\theta$ 角，然后绕

Y $Y$ 轴旋转

α $\alpha$ 角，最后绕

Z $Z$ 轴旋转

β $\beta$ 角。

基本原理

假设用 $Q_i,i=1,2,3,\cdots$ 表示第一个点集， $P_i,i=1,2,3,\cdots$ 表示第二个点集。两个点集的对齐配准转换为使下列目标函数最小。

E = \sum i = 1 n | | Q i - (P i \cdot R + T) | | 2 = m i n

$E=\sum_{i=1}^n||Q_i-(P_i\cdot R+T)||^2=min$

其中，旋转矩阵 $R$ 和平移矩阵 $T$ ，就是找到的待配准点云数据与参考点云数据之间的旋转参数和平移参数,使得两点集数据之间满足某种度量准则下的最优匹配。

ICP配准过程：
1. 计算 $P$ 中的每一个点在 $Q$ 点集中的对应最近点
2. 求得使上述对应点对平均距离最小的刚体变换，求得平移参数和旋转参数
3. 对 $P$ 使用上一步求得的平移和旋转参数，得到新的变换点集 $P'$
4. 如果新的变换点集与参考点集满足上面目标函数要求，即两点集的平均距离小于某一给定阈值，则停止迭代计算，否则新的变换点集 $P'$ 作为新的 P <script type="math/tex" id="MathJax-Element-35">P</script>继续迭代，直到达到目标函数的要求。

提示：这里的目标函数的要求，也可以是迭代次数。

测试

PCL官网手册中ICP例子使用的是monkey模型，这里同样使用monkey点云。下图中，左侧猴子为原始模型，右侧猴子是自己旋转平移后的目标猴子模型。接下来的实验将右侧猴子进行旋转平移和左侧猴子进行匹配。

左侧原始模型，右侧经过变换的模型

PCL ICP算法配准中的代码，使用一个输入模型作为原始模型A，通过变换（旋转和平移）原始模型得到一个新的模型B，将B模型使用ICP算法配准到A模型上。下图是PCL中源码未经修改运行出来的结果。

现在对PCL ICP配准源码稍作修改，这时候输入两个点云数据，参数1为原始模型，参数2为转换后的模型，参数3为重复次数。下图实验是再Blender中保存的两个模型。

这里的配准效果还不错
这里写图片描述

stanford bunny实验，数据量有点大，配准效率较低，但效果不错。

提示：配准效果和点集初始位置关系很大，博文中实验数据的初始位置都比较合适。

PCL数据官方数据下载

https://github.com/PointCloudLibrary/data
博文中实验的数据（猴子模型）从Blender中制作导出。

参考

http://pointclouds.org/documentation/tutorials/registration_api.php#registration-api
https://en.wikipedia.org/wiki/Iterative_closest_point
http://www.cnblogs.com/sddai/p/6129437.html
Chen, Yang; Gerard Medioni (1991). “Object modelling by registration of multiple range images”. Image Vision Comput. Newton, MA, USA: Butterworth-Heinemann: 145–155. doi:10.1016/0262-8856(92)90066-C
Besl, Paul J.; N.D. McKay (1992). “A Method for Registration of 3-D Shapes”. IEEE Trans. on Pattern Analysis and Machine Intelligence. Los Alamitos, CA, USA: IEEE Computer Society. 14 (2): 239–256. doi:10.1109/34.121791.
https://cn.mathworks.com/matlabcentral/fileexchange/27804-iterative-closest-point?requestedDomain=www.mathworks.com
http://www2.imm.dtu.dk/~jakw/publications/bscthesis.pdf
http://blog.sina.com.cn/s/blog_3fd642cf0101cc8w.html
https://en.wikipedia.org/wiki/Rotation_matrix
http://www.cnblogs.com/yuzhongwusan/p/4121815.html