ICP迭代最近点算法

1. 1. 三维点云配准
   1. 1.1 三维配准理论基础
   2. 1.2 ICP算法
2. 2. 二维轮廓点配准

1. 三维点云配准

1.1 三维配准理论基础

假设通过RGB-D相机得到了第一组点云$P=\{p_1, p_2,\cdots,p_n\}$，相机通过位姿变换（旋转+平移）后拍摄第二组点云$Q=\{q_1, q_2, \cdots, q_m\}$，其中$P_{xyz}$和$Q_{xyz}$分别对应两个独立的坐标系。假设相机的平移量为$t_{3 \times 1}=\{d_x, d_y, d_z\}^T$，旋转量用绕$y, x, z$轴的旋转角$\varphi, \omega, \kappa$来表示（先绕，再绕，最后绕，方向为逆时针），则空间点在两个坐标系下的坐标$p_i = \{x_i, y_i, z_i\}^T$与$q_i=\{x_i', y_i', z_i'\}^T$的对应关系为：

$$p_i = R_{3 \times 3}q_i+t_{3 \times 1}$$

其中，的构成如下：

$$R = \begin{bmatrix}a_{1} & a_{2} & a_{3}\\b_{1} & b_{2} & b_{3}\\c_{1} & c_{2} & c_{3}\end{bmatrix} = \begin{bmatrix}\cos\varphi & 0 & -\sin\varphi \\ 0 & 1 & 0 \\ \sin\varphi & 0 & \cos\varphi\end{bmatrix} \begin{bmatrix}1 & 0 & 0 \\ 0 & \cos\omega & -\sin\omega \\ 0 & \sin\omega & \cos\omega\end{bmatrix} \begin{bmatrix}\cos\kappa & -\sin\kappa & 0 \\ \sin\kappa & \cos\kappa & 0 \\ 0 & 0 & 1\end{bmatrix}$$

右手坐标系产生绕轴顺时针旋转和正方向平移后，导致变换为，那么变换关系应该从变换后的坐标往回计算，即：。可以通过二维右手系自行推导，右手系即右手大拇指、食指、中指展开后，分别代表轴。

用函数表示点集在变换下与目标点集的误差：

$$v(R, t) = \sum^n_{i=1}||Rq_i + t - p_i||^2$$

求解最优变换参数的问题可以转化为满足最优解的问题。

1.2 ICP算法

ICP：Iterative Closet Point，迭代最近点算法，分别在待匹配的目标点云Q和源点云P中，按照一定的约束条件，找出一定数量的最邻近的对应点，计算出最优变换参数，使得误差函数最小。其中误差函数为：

$$v(R,t) = \frac{1}{n}\sum^n_{i=1}||Rq_i+t-p_i||^2$$

为距离最近的对应点对的数量，为三维空间旋转矩阵，为三维空间平移向量，和分别是对应点在源点云坐标系和目标点云坐标系下的三维坐标。

算法流程：

1. 在目标点云中选取点集；
2. 根据最小距离准则在源点云中寻找对应点（需要去重心化处理）；
3. 计算旋转矩阵和平移向量；
4. 对进行三维运算，得到新的坐标
5. 计算与的平均距离；
6. 如果满足收敛条件，则停止迭代；否则令，返回第2步。

2. 二维轮廓点配准

ICP算法同样适用于二维情形：当相机对某个物体拍照得到影像后，对影像提取轮廓点得到边缘点云P；相机在同一空间平面内作二维旋转和平移，对同一物体拍照，提取轮廓点后得到边缘点云Q。在不考虑中心投影引起的变形时，轮廓点的配准可以采用ICP算法。

将ICP算法应用于二维情景通常针对相机姿态变化较小的情形，例如运动相机的相邻帧跟踪。