今天讲一篇关于利用SVD方法求解ICP问题的文献《Least-Squares Rigid Motion Using SVD》，这篇文章非常精彩地推导出将点对齐问题的解析解，同时总结了求解该问题的统一范式。

问题描述

已知以及是空间中（文中说的更加普适，，可以表示维空间）的匹配点集，我们试图找到这样的旋转矩阵和平移向量最小化如下对齐误差（即ICP问题的形式）：

接下来文章分别推导了平移向量以及旋转矩阵的解析解。

计算平移量

此时假定旋转矩阵是固定的，令

我们可以通过对求导的方式得到平移量的最优解，如下：

令：

于是我们得到的解：

从上式看出最优的平移量将点集的加权中心映射到了点集的中心。接下来将上式带入优化方程，得：

由此我们将原问题转换成了无平移量的优化问题，令：

我们把问题简写成如下形式：

计算旋转量

简化上式：

又因为旋转矩阵的正交性：；另外是标量：维度为，维度为，维度为。于是有下式：

得：

将整理好的上式带入简化后的优化问题，得：

接下来将要利用到如下关于迹的技巧:

上式就是对

的完美解释。

利用上式，式可以整理得：

这里说明一下维度：维度为，维度为，维度为，维度为。

接下来回顾一下迹的性质：，因此有下式：

令的”covariance”矩阵，求的SVD分解：

于是式变为：

由于均为正交矩阵，因此也是正交阵，也就是说的列向量是互相正交的单位向量，即：

于是：

由于SVD分解的性质可知的元素均为非负数：，于是式变为如下形式：

可见，当迹最大时，又由于是正交阵，这使得为单位阵！

看到没，R的解析解竟然如此简单，并且与SVD分解产生了联系，让人感觉到了数学的美妙。不过到这里还没完，后面作者进行了一步方向矫正，大意是这样的：利用公式得到的矩阵并不一定是一个旋转矩阵，也可能为反射矩阵，此时可以通过验证的行列式来判断到底是旋转（行列式 = 1）还是反射（行列式 = -1）。但我们要求的是旋转矩阵，这时需要对公式进行一步处理。

假设，则限制为旋转就意味着为反射矩阵，于是我们试图找到一个反射矩阵最大化下式：

即是以为变量的线性函数，由于，其极大值肯定在其定义域的边界处。于是当时，取得极大值，但是此时的为反射矩阵，所以并不能这样取值。然后我们看第二个极大值点，有：

这个值大于任何其它的自变量取值的组合（除了），因为奇异值是经过排序的，是最小的一个奇异值。

综上，为了将解转换为旋转矩阵要进行如下处理：

可以总结的套路

为了得到ICP问题的最优解，我们可以采取如下套路：

step1. 计算两组匹配点的加权中心：

step2. 得到去中心化的点集：

step3. 计算的covariance矩阵：

其中，为的矩阵，分别是它们的列元素，另外。

step4. 对进行SVD分解，得到旋转矩阵：

step5. 计算平移量：