ICP（iterative closest point）

迭代最近点求解

ICP的求解有两种方法，第一种是线性代数的（SVD分解），另一种是非线性优化的(BA)求解。

SVD方法

ICP问题是先知道若干匹配好的点P与P’的关系，然后进行求解，我们知道，没对匹配点之间的误差为：
以误差构建最小二乘，求可以使最小二乘最小的旋转矩阵R和平移向量t：

定义两组点的质心：

注意，质心是没有下标的，随后可以处理，我们先知道矩阵的二范数的定义，可以推导处如下的式子：

交叉项中，左边的式子求和后为零，这与式子中质心的求导有关，可以得到目标函数：

通过观察左右两项，我们发现左边只和旋转矩阵R相关，而右边有R和t但是只和质心位置相关，通过计算两组点的质心位置，可以计算每个点的去质心坐标：

进而可以求出旋转矩阵R：

将右边的式子等于零，可以计算平移向量：

注意

注意看，用SVD求解时主要是求解旋转矩阵R，我们就重点来计算以下旋转矩阵：

在上式中，此最小二乘只与最后一项有关，可以计算：

而SVD分解是先设一个矩阵为：

令：

而R为：