寻找两组点之间的最优欧氏变换

本文的原文来自 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 计算位移向量

这个就非常简单了: