找到相应3D点之间的最佳拟合

ljm1200

已于 2022-11-26 15:10:44 修改

阅读量797

点赞数

文章标签： 3d

于 2022-11-26 15:09:51 首次发布

原文链接：http://nghiaho.com/?page_id=671

版权

Finding optimal rotation and translation between corresponding 3D points | Nghia Ho

找到两组对应的3D点数据之间的最佳旋转和平移，以便它们对齐，这是我遇到的常见问题。下面显示了3个对应点（需要解决的最小点）的最简单情况下的问题说明。

对应的点具有相同的颜色，R是旋转，t是平移。我们希望找到将数据集A中的点与数据集B中的点对齐的最佳旋转和平移。这里，“最优”或“最佳”是指最小二乘误差。这种变换有时称为欧几里得变换或刚性变换，因为它保留了形状和大小。这与仿射变换形成对比，仿射变换包括缩放和剪切。

该问题尤其出现在3D点云数据找正等任务中，其中数据是从3D激光扫描仪或流行的Kinect设备等硬件获得的。

解决方案概述

所提出的解决方案将适用于无噪声和有噪声数据。对于唯一解至少需要3个点。

对于无噪声数据，它将解决以下问题：

$RA+t=B$

如果数据有噪声，它将最小化最小二乘误差

$err = \sum_{i=1}^{N}||RA_i+t-B_i||^2$

其中A和B是具有已知对应性的3D点的集合。R是3×3旋转矩阵，t是平移向量（技术上是矩阵Nx3）。

找到最佳刚性变换矩阵可以分解为以下步骤：

1.找到两个数据集的质心
2.将两个数据质心设置到原点上，然后找到最佳旋转R
3.找到平移t

寻找质心

该方法很简单，质心仅为点的平均值，可计算如下：

$centroid_A = \frac{1}{N} \sum_{i=1}^{N}A_i$

$centroid_B = \frac{1}{N} \sum_{i=1}^{N}B_i$

的矢量，例如：

$\begin{bmatrix} x \\ y\\ z\\ \end{bmatrix}$

寻找最佳旋转

有几种方法可以找到点之间的最佳旋转。我发现最简单的方法是使用奇异值分解（SVD），因为它是一个在许多编程语言（Matlab、Octave、使用LAPACK的C、使用OpenCV的C++等）中广泛可用的函数。SVD就像线性代数中的一根强大的魔杖，可以解决各种数值问题，但不幸的是，我在大学的时候没有教过SVD。我不会详细介绍它是如何工作的，而是如何使用它。您只需要知道SVD将一个矩阵（称为E）分解为3个其他矩阵，例如：

$[U,S,V] = SVD(E)$

$E = USV^T$

如果E是一个方阵，那么U、S和V的大小也是相同的。我们只对这个方阵感兴趣，所以我不会详细讨论每个矩阵。

为了找到最佳旋转，我们首先重新调整两个数据集的中心，使两个质心位于原点，如下所示。

这将去除平移成份，只需要处理旋转。下一步得到一个累积矩阵，一个称为H的矩阵，并使用SVD找到旋转，如下所示：

$H = (A-centroid_A)(B-centroid)^T$

$[U,S,V] = SVD(H)$

$R = VU^T$

H是熟悉的协方差矩阵。是一种用中每列减去的运算。

在这一点上，你可能会想，“什么，那么容易？！”，事实上，你是对的。需要注意的一点是，您正确地计算了H。它最终应该是一个3×3矩阵，而不是一个NxN矩阵。请密切注意转置符号。它在两个矩阵之间进行乘法，其中维数分别为3xN和Nx3。乘法的顺序也很重要，如果以另一种方式进行，则会发现从B到A的旋转。

案例的特殊反思

在寻找旋转矩阵时，有一个特殊的情况，需要注意。有时SVD会返回一个反射矩阵，这在数值上是正确的，但在现实生活中实际上是无意义的。这可以通过检查R的行列式（来自上面的SVD）并查看它是否为负（-1）来解决。如果是，则V的第三列乘以-1。

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

计算平移t

现在我们已经解出了R，我们可以解出t。将质心代入开头提到的方程中，我们得到：

$R \times A + t = B$

$R \times centroid_A + t = centroid_B$

$t = centroid_B - R \times centroid_A$

用法说明

提出的解决方案可以用于任何大小的数据集，只要至少有3个点。当存在多于3个点时，用最小最小二乘法求解。

Code

import numpy as np

# Input: expects 3xN matrix of points
# Returns R,t
# R = 3x3 rotation matrix
# t = 3x1 column vector

def rigid_transform_3D(A, B):
    assert A.shape == B.shape

    num_rows, num_cols = A.shape
    if num_rows != 3:
        raise Exception(f"matrix A is not 3xN, it is {num_rows}x{num_cols}")

    num_rows, num_cols = B.shape
    if num_rows != 3:
        raise Exception(f"matrix B is not 3xN, it is {num_rows}x{num_cols}")

    # find mean column wise
    centroid_A = np.mean(A, axis=1)
    centroid_B = np.mean(B, axis=1)

    # ensure centroids are 3x1
    centroid_A = centroid_A.reshape(-1, 1)
    centroid_B = centroid_B.reshape(-1, 1)

    # subtract mean
    Am = A - centroid_A
    Bm = B - centroid_B

    H = Am @ np.transpose(Bm)

    # sanity check
    #if linalg.matrix_rank(H) < 3:
    #    raise ValueError("rank of H = {}, expecting 3".format(linalg.matrix_rank(H)))

    # find rotation
    U, S, Vt = np.linalg.svd(H)
    R = Vt.T @ U.T

    # special reflection case
    if np.linalg.det(R) < 0:
        print("det(R) < R, reflection detected!, correcting for it ...")
        Vt[2,:] *= -1
        R = Vt.T @ U.T

    t = -R @ centroid_A + centroid_B

    return R,