本文的原文来自 Finding optimal rotation and translation between corresponding 3D points

这里我们要达成的目标是给定两组点，要尝试找到最优的旋转+位移操作，将这两组点叠在一起，而误差最小。如下图所示。

图中的颜色代表了点之间的对应关系。 代表了旋转操作，而 代表了位移操作。在最优操作下，两组点之间的位置误差的 MSE 最小。这种变换被称为 [Eaclidean] 或者 [Rigid] 变换。变换保留了形状特征和大小特征。不同于仿射变换，仿射变换还包括了缩放与剪切变换。

1 解法概览

这里涉及的求解方法会涉及两种情况：有噪声和无噪声。至少需要三个点才能得到唯一解。

对于无噪声环境，求解过程等价于解方程

对于有噪声的环境，求解过程为最小化下面的误差：

在三维空间中， 是一个 矩阵。 则是一个三维向量。

求解完整的变化过程可以分解为下面三个步骤：

  1. 寻找两个数据集的重心(centroids)
  2. 将两组数据都放到原点来寻找最优的旋转
  3. 寻找最优的

2 寻找重心

这个很简单：

3 计算最优旋转

有不少寻找最优旋转的方法，其中最简单的一种是基于奇异值分解（Singular Value Decomposition, SVD）。这部分的数学原理细节就不说了，不知道的同学找一本线性代数的教材吧。SVD 将一个矩阵分解为三个矩阵的乘积：

其中 和 都是方阵。为了找到最优旋转我们先将两组点的重心都放到原点：

这样可以让我们独立于位移操作只考虑旋转。我们首先计算这样的矩阵：

对这个矩阵做奇异值分解，得到 ，那么旋转矩阵就是

这里的 应该是一个协方差矩阵，其大小是 ，而不是 。另外，乘的顺序也很重要。如果将乘的顺序反过来会得到从 B 旋转到 A 的矩阵。

4 特殊的反射情况

在一些特殊的情况下，上面的方法可能找到一个反射矩阵。这个解在数值上是正确的，但是在实际场景中没有意义。我们可以检查 矩阵的行列式来判断。如果 的行列式是否是负数，如果是，就将 的第三列乘以 -1。

1
2
3
4
5
if determinant(R) < 0
    [U,S,V] = svd(R)
    multiply 3rd column of V by -1
    R = V * transpose(U)
end if

5 计算位移向量

这个就非常简单了：