拟合三维空间一组点云到另一组点云的刚体变换之二：SVD方法

假设对应关系已知，拟合求得一组三维点云到另一组三维点云的刚体变换矩阵。

高翔大神在《视觉SLAM十四讲》中介绍了一个效率很高的3D点云对齐算法。假设点云对应关系是已知的，这个方法先求解旋转 $R$ ，再求解平移 $t$ 。

假设有两组点云 $P=\left \{ p_1, p_2, p_3, ..., p_n \right \}$ 和 ${P}'=\left \{ {p}'_1, {p}'_2, {p}'_3, ... , {p}'_n \right \}$ ，求 ${P}'$ 到 $P$ 的变换 $R$ 和 $t$ ，使满足

$\forall_i, p_i = R{p}'_i+t$

这个问题分为三个简单步骤：

转化为去质心坐标。令 $p$ 和 ${p}'$ 分别为 $P$ 和 ${P}'$ 的质心（即坐标算数平均值），去质心坐标 $q_i = p_i - p$ ， ${q}'_i = {p}'_i - {p}'$ 。
求解旋转矩阵 $R$ 。这是最微妙的部分，其具体原理在[1]和[2]中有介绍。我们令 $W=\sum_i{q_i{{q}'_i}^T}$ ，这里 $W$ 显然是一个3*3的矩阵。接下来对 $W$ 进行SVD分解，得到 $W=U \Sigma V^T$ 。当 $W$ 满秩时，则 $R=U V^T$ 。
求解平移 $t=p-R{p}'$ 。

与这里描述的naive方法相比，SVD方法时间复杂度为O(n)，相关实现在Eigen中也有提供，十分适合实际应用。

P.S.: 第2步求解的 $W$ 应该表示的是 ${P}'$ 到 $P$ 的一个自由变换，参照以前的这篇文章可以看出，其SVD结果 $\Sigma$ 对角线上各个元素表示的是其在三组正交基上的放缩比，我们直接取 $R=U V^T$ 即是认为它在各个基上的放缩比为1（即不放缩），从而得到了一个满足需要的刚体变换。

[1]K. S. Arun, T. S. Huang, and S. D. Blostein, “Least-squares fitting of two 3-d point sets,” PatternAnalysisandMachineIntelligence, IEEETransactionson,no.5,pp.698–700,1987.

[2]F. Pomerleau, F. Colas, and R. Siegwart, “A review of point cloud registration algorithms for mobile robotics,” Foundations and Trends in Robotics (FnTROB), vol. 4, no. 1, pp. 1– 104, 2015.