如何实现点到面距离ICP

Question

我正在实施 2D ICP（迭代最近点）。我尝试过使用点对点距离但是损失很大

事实上，两个相邻帧之间的损失是可以接受的，但是，例如，当我计算第 1 帧和第 20 帧之间的损失时，它会变得很大。

我正在尝试用点对平面替换点对点，但我不知道如何实现。 对于点对点，我可以使用 SVD，如下所示：

        # Compute the centers of both point clouds
        p = np.mat([np.average(left1_x), np.average(left1_y)], dtype=np.float64)  # Last frame
        q = np.mat([np.average(left2_x), np.average(left2_y)], dtype=np.float64)  # Current frame

        # Compute the matrix
        Q = np.zeros((2, 2), dtype=np.float64)
        for j in range(len(nearest)):
            Q += np.matmul(np.mat([left2_x[j], left2_y[j]]).T - q.T, np.mat([left1_x[j], left1_y[j]]) - p)
        U, _, V_T = np.linalg.svd(Q)

        # Rotation
        R = np.matmul(V_T, U.T)

        # Translation
        t = p.T - np.matmul(R, q.T)

但是对于点到平面，如何从点云中找到平面以及如何计算旋转和平移矩阵？

Answer 1

我认为你可以使用最小二乘法来找到一般变换。假设您有源点云和目标点云以及一个返回对应列表的函数（基本上是一个 KDTree）。然后迭代每个点并解决最小二乘问题，您将得到最佳的变换矩阵。然后分解旋转和平移。

correspondences = find_correspondences(source_points, target_points, target_normals)
A = []
b = []

for src_point, (target_normal, target_point) in zip(source_points, correspondences):
    A_row = np.cross(src_point, target_normal)
    A.append(A_row)
    b.append(np.dot(target_normal, target_point - src_point))

A = np.array(A)
b = np.array(b)

# Solve for transformation using linear least squares
x, _, _, _ = np.linalg.lstsq(A, b, rcond=None)

如果你想了解更多，请看原始点到平面ICP论文。