ICP算法公式推导

一、引言

ICP算法可以用于匹配两个点云，得到两个点云之间的位姿转换关系。本文主要介绍已知对应点的ICP匹配算法。

二、ICP算法

首先计算两个点云的质心：

然后将原始点云减去对应的质心，得到去除质心的点云：

计算新产生的两个点云形成的W矩阵，并SVD分解：

那么，两个原始点云的转换关系R和t如下所示：

三、推导过程

对于两组点云：

求解R和t，使得下式最小：

上式进行变换：

其中，цx 和 цp分别为X点云和P点云的质心。
转换为去除质心的形式：

展开得到

由于后面两项都有

我们可以令该项等于0，然后式子转化为

该式子仅与R有关，可以寻找可以最小化上式的R，然后根据=0求解t。

上式进一步转化，得到：

其中，X’ 和 P‘ 是原始点云减去对应质心的点点云，x’和p’是其中的点。

展开得到：

其中旋转矩阵R因为是正交矩阵，所以RTR=1，因此求上式的最小值等价于求最后一项的最小值：

即该式的最大值：

进行转化，用矩阵的迹进行表示：

其中，

对H进行SVD分解得到：

设定一个变量X为

得到：

此时得到的XH是一个正定对称矩阵

根据定理：

假设矩阵A为正定对称矩阵，则对于任意的正交矩阵B，都有

类比到我们的推导这里，我们准备求解可以让Trace(RH) 最大的R值，

我们发现，当R=VUT的时候，RH的结果是一个正定对称矩阵

而对于任意一个正定对称矩阵来说，根据定理可以知道，我们再乘以任何一个正交矩阵（旋转矩阵是正交矩阵），都会导致Trace值下降

所以说明，此时的R=VUT取值可以让Trace(RH)取得最大值。

因此：


参考文献：
Least-Squares Fitting of Two 3-D Point Sets