ICP Mathematical Derivations Step by Step

1. Problem Formulation

Given two point sets and , if is transformed from rigidly as , we can solve the unknown transformation composed by the rotation matrix and the translation vector . Such problem is often seen as the point cloud registration problem, and the most important method to handle this type of problems is called the Iterative Closest Point (ICP) method. In this note, we will derive the problem step by step, both for the closed-form and the optimization-based solutions.

2. Point-Point ICP

As formulated in the previous chapter, the data association is provided and denoted for each point pair with footnote . Usually in ICP, the point association is determined via nearest neighbor searching methods. We simplify the data association and concentrate on the registration.

2.1. Direct Solution

Calculate the centroids of and as and . We have the loss function derived as

Breaking down the quadratic term into

Since the cross-term includes and , the product of the cross-term is .

Therefore,

To find optimal , we take the derivative of :

Take the optimal in the objective function, we have

The first and second terms do not relate to , and due to , is canceled in the first squared term. Thus, we have

Let , which is the cross covariance matrix of centralized point sets and . We can rewrite the objective function as

Perform sigular value decomposition of , we get

Accordingly, the objective function is

Since are orthogonal (therefore is orthogonal) and is diagonal, according to Cauchy-Swarchz inequality,

Therefore, the optimal is found when the above inequality meets the equal condition, i.e.,

The optimal rotation matrix

Note: In some literature you might see . Please check whether the matrix is composed in another way round as .

2.2. Non-linear Optimization Approaches

Although the standard form of the ICP objective function can be solved using the previously derived closed-form solution, it is important to also introduce optimization-based methods. These methods are highly useful for various variants of the standard ICP, enabling better solutions of specific types of registration problems. Below, we introduce the standard ICP problem solved via Gauss-Newton algorithm.

Recap of the least squares formulation of the problem

Denote , where is the Euclidean residual vector for point pair .

According to Gauss-Newton equation,

where is the Jacobian of the rotation and translation. As is an orthogonal matrix, the constrained optimization problem is difficult to solve. Therefore, we utilize the rotation vector as the unknown rotation variable, and take the Jacobian of the Lie algebra accordingly.

Note that the exponential map gives The notation is the skew-symmetric conversion of a vector to a square matrix.

Given that the rotation is described in the angle-axis form of 3 variables, and the translation is also 3 dimentional, the Jacobian of each term is , composed by the rotation part and the translation part. The full Jacobian is of size stacked by total numbers of .

As a result, the optimal step size for Gauss-Newton iteration is solved via equation (1). The final transformation parameters can be iteratively optimized via .