Iterative Closest Point algorithm. Nearest Neighbours and Point-to-Plane.

ICP: Introduction

Code: https://colab.research.google.com/drive/1H1Kg-a9G-doOjueXrAYoAUh0ivnywEjw?usp=sharing

ICP(Iterative Closest Point) algorithm is one of the most famous algorithms to minimize or understand the differences between two point clouds. It’s often used for point cloud registration to merge multiple scans and also to estimate rigid transforms between them for building maps for robotic applications, due to is simplicity and effectiveness.

There are many variants of the ICP algorithm, but all of them can be decomposed into similar components, a target point cloud(acts as a reference), a source point cloud(which is to be transformed) and method to measure correspondences between source and target. There is also usually a step to filter outliers using RANSAC(or other methods).

We are going to start with one of the simplest implementations. In this implementation nearest neighbor of each point in the target(reference) point cloud in source point cloud will be used as correspondences.

ICP is an iterative algorithm where in each step, the point cloud alignment is performed and error is computed, if error is less than a predefined threshold the iteration is terminated otherwise the source point cloud is transformed and the iteration continues.

Computing the error

The error measure is used to terminate the ICP, for our implementation the error is measured as the mean of norm of distances between the target points and corresponding transformed source points, the equation can be seen in Figure-1.

Estimating the relative transform

The goal of ICP is to estimate the relative transform (rigid) between target and source point clouds. This transform is calculated by first establishing correspondences between the two sets of points. These correspondences are then used to estimate the relative transform.

The Rotation Matrix and translation vector are can be estimated using equations 1–4. The proof of this closed form solution is out of the scope for this blog and can be found in [1].

Figure-1: Equations for vanilla ICP. Error computation, and closed form transform estimation (1–4).

Initializing the parameters

The parameters to be estimated i.e. R and t have to be initialized for the first iteration of ICP. There are many possible ways to initialize these parameters and the choice usually depends on the knowledge of the use case at hand. However, a good starting point if initializing R as an identity matrix and t has the difference between coordinates of center of mass for target and source clouds.

import numpy as np
import open3d import o3d

# Read demo point clouds
demo_icp_pcds = o3d.data.DemoICPPointClouds()
source = o3d.io.read_point_cloud(demo_icp_pcds.paths[0])
target = o3d.io.read_point_cloud(demo_icp_pcds.paths[1])

# Initialize R and identity and t as diff in mean coordinates
R = np.eye(3)
t = np.mean(np.asarray(target.points), axis=0) - np.mean(np.asarray(source.points), axis=0)

Writing the step function

Let’s now write the step function which would return the error and estimated transform after each iteration of the algorithm. The error is computed at each step using a matching function. The step-function described next uses nearest neighbors to estimate the transform and corresponding error.

def nearest_neighbour_matcher(source: PointCloud, target: PointCloud, **kwargs) -> Tuple[float, NDArray]:
    source_correspondences, _ = find_nearest_neighbors(source_pc=source,
                                                        target_pc=target,
                                                        nearest_neigh_num=1)
    source_centroids = np.mean(source_correspondences, axis=0)
    centre_adjusted_source = source_correspondences - source_centroids

    # SVD to obtain closed form solution
    target_centroids = np.mean(np.asarray(target.points), axis=0)
    centre_adjusted_target = np.asarray(target.points) - target_centroids
    covariace_matrix = centre_adjusted_target.T @ centre_adjusted_source
    # My intuitive understanding is extracting the rotation 
    # components of the covariance matrix, if the initial differences 
    # are not very large the covariance matrix already reveals how related
    # the source and targets are. Now treating this matrix as an affine 
    # transformation the rotation components from the SVD should be rotating 
    # any input into a coordinate space where both source and target agree "kind-of".
    # I may be wrong.
    U, X, V_T = np.linalg.svd(covariace_matrix)
    R = U @ V_T
    t = target_centroids - (R @ source_centroids)

    # No need for t in this error as target and soource are centre adjusted
    error = np.mean(np.linalg.norm(centre_adjusted_target - (R @ centre_adjusted_source.T).T, axis=0))

    transform = np.hstack((R, t.reshape((3, 1))))
    transform = np.vstack((transform, np.array([[0, 0, 0, 1]])))
    return error, transform

The find_nearest_neighbors function is implemented as shown here.

import open3d as o3d
from open3d.geometry import PointCloud

def find_nearest_neighbors(source_pc: PointCloud, target_pc: PointCloud, nearest_neigh_num: int):
    # Find the closest neighbor for each anchor point through KDTree
    point_cloud_tree = o3d.geometry.KDTreeFlann(source_pc)
    # Find nearest target_point neighbor index
    points_arr = []
    indices = []
    for point in target_pc.points:
        [_, idx, _] = point_cloud_tree.search_knn_vector_3d(point, nearest_neigh_num)
        points_arr.append(source_pc.points[idx[0]])
        indices.append(idx[0])
    return np.asarray(points_arr), np.array(indices)

Putting it all together

The overall flow of the algorithm is described in the flowchart in Figure-2.

Figure-2: Overall flow for ICP algorithms.

The nearest neighbor method of finding correspondences gives us quite accurate results, however it’s prone to noise and uses too little information by doing one to one matching. Also the convergence is quite noisy and it may take a while to converge.

The natural next choice is to try to Point-to-Plane method which is more accurate and converges quicker than nearest neighbors(but each iteration is slower as compared to nearest neighbors).

Point to plane ICP

Point to plane ICP is another variant of ICP where instead of using nearest neighbors (point-to-point error) method the correspondences are established using a virtual plane at each source point and computing its distance from target (destination) point.

The point to plane ICP tries to minimize the error in Equation-4. As can be seen the matrix M has non-linearities and the equation is minimized using non-linear least-squares methods like the Levenberg–Marquardt method.

However if we know that the relative orientation is small enough the problem can be approximated to a linear least-squares optimization problem2.

The step-function is now changed to the function detailed below.

def linear_point_to_plane_matcher(source: PointCloud, target: PointCloud, **kwargs):
    # The algorithm used here and the linearity assumption is described here:
    # https://www.comp.nus.edu.sg/~lowkl/publications/lowk_point-to-plane_icp_techrep.pdf
    def linear_model(params, A, b):
        rx = params['rx']
        ry = params['ry']
        rz = params['rz']
        tx = params['tx']
        ty = params['ty']
        tz = params['tz']
        x = np.array([rx, ry, rz, tx, ty, tz])
        b_hat = np.dot(A, x)
        return b_hat - b

    def get_transform_matrix_from_params(fitted_params):
        # Rotations/translations along
        # x, y, z axes respectively.
        rx = fitted_params['rx'].value
        ry = fitted_params['ry'].value
        rz = fitted_params['rz'].value
        tx = fitted_params['tx'].value
        ty = fitted_params['ty'].value
        tz = fitted_params['tz'].value
        # For a small enough theta. sine(theta) approximated as theta
        # and cos(theta) as 1.
        T = [
                [  1  ,  rx*ry - rz  , rx*rz + ry  , tx  ],
                [  rz , rx*ry*rz + 1 , ry*rz - rx  , ty  ],
                [ -ry ,      rx      ,      1      , tz  ],
                [  0  ,       0      ,      0      ,  1  ]
            ]
        return T

    source.estimate_normals()
    source_points, indices = find_nearest_neighbors(source, target, nearest_neigh_num=1)
    normals = np.asarray(source.normals)[indices]

    # The intuition here is quite simple. Estimate a plane at target points
    # minimize the distance of this plane from corresponding source point.

    A = np.concatenate(
                        (
                            np.cross(source_points, normals),
                            normals
                        ),
                        axis=1
                      )
    b = np.sum((np.asarray(target.points) - source_points) * normals, axis=1)

    params = kwargs["params"]
    min_result = lmfit.minimize(linear_model, params, args=(A, b,), method='least_squares')
    params = min_result.params
    params_arr = np.array([params["rx"].value, params["ry"].value, params["rz"].value, params["tx"].value, params["ty"].value, params["tz"].value])
    error = np.mean(np.square(np.dot(A, params_arr) - b))
    transform = get_transform_matrix_from_params(params)

    return error,  transform

In Figure-3 the error from both variants of ICP are illustrated. The Point-to-Plane converged faster.

Figure-3: Error from both nearest-neighbour and point-to-plane ICP against the number of iterations before loop exit.

All the methods and code described in the blog can be found here.

[1]: K. S. Arun, T. S. Huang and S. D. Blostein, “Least-Squares Fitting of Two 3-D Point Sets,” in IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. PAMI-9, no. 5, pp. 698–700, Sept. 1987, doi: 10.1109/TPAMI.1987.4767965.