如何实现点平面距离的ICP

Question

我正在执行2D ICP(迭代最近点)。我试过使用点对点距离，但是损耗很大。

实际上，两个相邻帧之间的损耗是可以接受的，但是，例如，当我计算frame1和frame20之间的损耗时，它变得很大。

我试图用点对平面取代点对点，但我不知道如何实现它。对于点对点，我可以像下面这样使用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纸。