算法公式推导——ICP

前言：点云配准中的迭代最近点（Iterative Closest Points，ICP）算法的推导，网上很多资料都有介绍，但是对我来说，看完这些资料还是有写些太明白的地方，前段时间学习的相关课程中讲到了ICP的算法推导，感觉很详细，因此这里详细总结一下，确保任何一个有线性代数基础的小伙伴都能看懂（若由任何问题或写的不清楚的地方，欢迎大家提出并一起交流）。

1，算法输入

两堆点集： X = { x 1 , x 2 , . . . , x M } \mathbf{X} = \{x_1, x_2, ..., x_M\} X={ x1​,x2​,...,xM​} 和 Y = { y 1 , y 2 , . . . , y N } \mathbf{Y} = \{y_1, y_2, ..., y_N\} Y={ y1​,y2​,...,yN​}。$\mathbf{X}$和$\mathbf{Y}$是需要进行配准的两帧点云。首先，对点云 $\mathbf{Y}$ 中的每个点，找到其在点云$\mathbf{X}$中的对应点，最终得到与点云$\mathbf{Y}$中每个点挨个儿对应的点集 X ∗ = { x 1 ∗ , x 2 ∗ , . . . , x N ∗ } \mathbf{X^*} = \{x_1^*, x_2^*, ..., x_N^*\} X∗={ x1∗​,x2∗​,...,xN∗​}，其中$x_i^{\ast }\in \mathbf{X}$。

2，目标函数

对于已知对应点的点集 { ( x 1 ∗ , y 1 ) , ( x 2 ∗ , y 2 ) , . . . , ( x N ∗ , y N ) } \{(x_1^*, y_1),(x_2^*, y_2),... , (x_N^*, y_N) \} { (x1∗​,y1​),(x2∗​,y2​),...,(xN∗​,yN​)}，要找到一个旋转矩阵R和一个平移向量t，使得关于它们的目标函数$E(R,t)=\frac{1}{N}\sum _{i=1}^N||x_i^{\ast }-R{y}_i-t|{|}^2$ 最小。为了书写方便，后面用$x_i$代替$x_i^{\ast }$（但应注意这里的$x_i$不等于点集$\mathbf{X}$中尚未知对应点的$x_i$）。

【注：其实这里我一直很疑惑，这个对应点是怎么找的，把点集$\mathbf{X}$中距离点$y_i$最近的点，作为它的对应点吗？】

因此，目标函数：$minE(R,t)=min\frac{1}{N}\sum _{i=1}^N||x_i-R{y}_i-t|{|}^2$

3，理论推导

步骤1：

$$\begin{array}{rl}E(R,t)=& \frac{1}{N}\sum _{i=1}^N||x_i-R{y}_i-t|{|}^2\\ =& \frac{1}{N}\sum _{i=1}^N||x_i-R{y}_i-t-(u_x-R{u}_y)+(u_x-R{u}_y)|{|}^2\\ =& \frac{1}{N}\sum _{i=1}^N||(x_i-u_x)-R(y_i-u_y)+(u_x-R{u}_y-t)|{|}^2\\ =& \frac{1}{N}\sum _{i=1}^N(||(x_i-u_x)-R(y_i-u_y)|{|}^2+||(u_x-R{u}_y-t)|{|}^2+2(x_i-u_x-R(y_i-u_y){)}^T(u_x-R{u}_y-t))\\ =& \frac{1}{N}\sum _{i=1}^N(||(x_i-u_x)-R(y_i-u_y)|{|}^2+||(u_x-R{u}_y-t)|{|}^2)+2\cdot \frac{1}{N}\sum _{i=1}^N((x_i-u_x-R(y_i-u_y){)}^T)\cdot (u_x\\ =& \\ =& \\ =& \end{array}$$