How to implement ICP with point-to-plane distance

Question

I'm implementing 2D ICP(Iterative Closest Point). I tried to use point-to-point distance but the loss is large.

In fact, the loss between two adjacent frames is acceptable but, for example, when I calculate loss between frame1 and frame20 it becomes large.

I'm trying to replace point-to-point with point-to-plane, but I don't know how to implement it. For point-to-point, I can use SVD like below:

        # 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)

But for point-to-plane, how can I find the plane from the point cloud and how to calculate rotation and translation matrix?

score 0 · Answer 1 · answered Aug 17 '23 at 20:15

I think you can use least squares method to find the general transformation. Let's say you have source and target point clouds and a function that returns the correspondence list, basically a KDTree. Then iterating through each point and solving the least squares problem, you would get the best transformation matrix. Then decompose for rotation and translation.

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)

If you want to get more, take a look at original point-to-plane ICP paper.

How to implement ICP with point-to-plane distance

1 Answers1