‎"Behind every stack of books there is a flood of knowledge."

What is ICP: Iterative Closest Point?


Initially what ICP did seemed like magic to me, after spending some time I’ve gotten a slightly deeper level of understanding to de-mystify the magic behind the process. This is my attempt at simplifying ICP for others and to outline my understanding so far.

For this tutorial, we’ll do the following:

  • Understand what ICP is
  • Understand the different steps involved in ICP
  • What a transformation matrix looks like

I’m going to focus on explaining ICP with respect to point clouds. What are point clouds? They are basically a set of points in 3D space having X, Y and Z dimensions (as opposed to an image that is 2D and has only x,y information). Having this third dimension Z allows us to get a 3D representation of the scene or in simpler words we get depth information, we know how far on object is with respect to something else.

As a side note, point clouds can be obtained using various tools such as: laser scannerstime-of-flight cameras or using regular cameras and applying stereo vision techniques to the left and right images. However, this isn’t the focus of this article. We assume, that we already have generated point clouds.

Let us define the problem first. Lets assume that we have two point clouds of the same object. The only difference between the two point clouds is that one of them is transformed, either: rotated (rotated around the axis) or translated (moved along the axis) with respect to the original/first point cloud. We will call the original/first point cloud from now as the target point cloud and the transformed point cloud as the source point cloud.

Figure 1: Source cloud can be rotated or translated

The end-goal/solution is: to be able to move the second point cloud in such a way as to negate the change (either the rotation or translation) it has undergone and bring it back to where the original/first point cloud is positioned as shown in the overlapping convergence of the image below:

We can choose to do this using two techniques:

  • manually position/transform the second point cloud to the original position using trial/error techniques and visual information from the changes until we reach the initial position
  • Or implement an algorithm that automatically positions the second point cloud to its original position (via iteratively trying to keep improving the estimates) — this is where ICP comes in.

Figure 2: Source cloud converged on Model cloud after ICP

ICP is a registration technique that uses geometry information (X, Y, Z) and not intensity/color to register the source point cloud to the target point cloud. It does so iteratively as depicted above. The steps are roughly described below:

  • Initially the source cloud is completely mis-aligned with respect to the target cloud, after niterations the source cloud seems to be aligned slightly better in terms of rotation (getting there).
  • Next, after a few more iterations (n + k), the source is now almost completely aligned to the target cloud in terms of rotation (but not translation).
  • Finally after even more (n + k + t) iterations the source cloud completely overlaps the target cloud and is aligned in both translation and rotation.

ICP allows us to converge a source point cloud having 6 degrees of freedom (DoF) from the target point cloud. What are the 6 degrees of freedom?

  1. Translation about the x-axis
  2. Translation about the y-axis
  3. Translation about the z-axis
  4. Rotation about the x-axis (also called pitch)
  5. Rotation about the y-axis (also called yaw)
  6. Rotation about the z-axis (also called roll)

The following image from Wikipedia visually summarizes all the possible 6 DoF movements:

Figure 3: 6 Degrees of Freedom

Now, the obvious question must be what happens within the different iterations that leads to a better alignment? The quick answer is the minimization of an error function, but this gives us a great opportunity to discuss ICP from the ground-up.

Since we are talking about ICP with respect to point clouds, I am going to represent the target/source clouds with points instead so that it is easier to co-relate:

Figure 4: Point representation of target/source clouds

The first step in ICP matching between the target cloud to the source cloud is to determinecorrespondences between the two clouds. What that means is that we need to match a set of points in the source cloud to original points in the target cloud that have merely been transformed (rotated and/or translated). This is called the correspondence problem.

Ideally if we know all the correspondences between the two clouds, we can merely compute how those points in the source cloud are transformed from the target cloud and use the transform to register the entire point cloud.

Figure 5: Correspondences between target and source clouds

And if we use the transformation of merely those selected points, we get a theoretically perfect alignment:

Figure 6: Perfect transformation match between target and source clouds

Now that we know a little about correspondences, a few questions become obvious:

  • How do we know which set of points to select for the matching?

This I believe is the basis for ICP: it assumes that the closest points are the corresponding points.

How do you define closest? Well, it simply means the point on the target cloud which has the smallestEuclidean distance from a point on the source cloud. The explanation of Euclidean distance is if we had two points p (on the target cloud) and q (on the source cloud), the Euclidean distance would be the line segment connecting point p and q. The distance can be quantitatively calculated by the below formula (from Wikipedia). For point P and Q, in our point cloud (3D) case we have P (x,y,z) and Q (x,y,z): thus

  • p1 = x co-ordinate | q1 = x co-ordinate
  • p2 = y co-ordinate | q2 = y co-ordinate
  • pn = z co-ordinate | qn = z co-ordinate

Thus, the points selected for the matching are shown below (in green). Figure 6 shows the approximation compared with true correspondences whereas Figure 7 show only the Euclidean approximation:

Figure 6: Closest Euclidean distance points between target and source clouds

As can be seen from Figure 6, at-least for these example point clouds, making an approximation that the corresponding points might be those that have the smallest Euclidean distance between them isn’t a bad one as they are quite close to the true correspondences.

Thus, ICP now becomes a minimization problem. All we need to do is define what has to be minimized. As mentioned above, we would like to transform the source cloud relative to the target cloud, such that the sum of all point-wise Euclidean distances is minimized. From above, we are aware that the source cloud needs to be rotated and/or translated to be correctly aligned with the target cloud. Thus we conclude that the transformation \mathcal{T} is a function of a rotation and a translation. Using matrix algebra, a rotation in 3D space can be represented by a matrix multiplication of a 3D point \mathbf{x} with the rotation matrix R. A translation can be represented by the vector addition of a 3D point \mathbf{x} and a translation vector \mathbf{t}.

To fix notation, let there be N points in the subset of points chosen to be used in each ICP iteration. In other words, N points are chosen from each of the source cloud and the target cloud which are assumed to be in correspondence with each other. Let \mathbf{x}_i^{\text{source}} represent an arbitrary point in the source cloud and  \mathbf{x}_i^{\text{target}} represent its corresponding point in the target cloud.

We said that it is the source cloud that has to be transformed. We represent this transformation as

\[\mathcal{T}(\mathbf{x}_i^{\text{source}}) = R \mathbf{x}_i^{\text{source}} + \mathbf{t} \]

Now, if we want to compute the Euclidean distance between the target point and the source cloud point after transforming the source point:

\[\mathbf{x}_i^{\text{target}} - \mathcal{T}(\mathbf{x}_i^{\text{source}}) = \mathbf{x}_i^{\text{target}} - (R \mathbf{x}_i^{\text{source}} + \mathbf{t})\]

For the entire point cloud, we can compute the total error in the cloud alignment using a transformation \mathcal{T} by summing over all the points in the subset considered. This forms the error function f(R,\mathbf{t}) that we wish to minimize

\[\text{arg} \min_{R,\matbf{t}} f(R,\mathbf{t}) = \frac{1}{N} \sum_{i=1}^{N} \mathbf{x}_i^{\text{target}} - (R \mathbf{x}_i^{\text{source}} + \mathbf{t})\]

All we are saying is that for each point from the target cloud-source cloud pair we consider, take the Euclidean distance, square it and do that for all the other points in the sub-set and take the mean. This gives us a mean, squared error function that is based on rotation and translation. This is what ICP is minimizing!

Figure 7: Closest euclidean distance points between target and source clouds

  • How many points do we select and which specific points?

To understand this question, we need to understand Random Sample Concensus (RANSAC). RANSAC is an iterative technique by which we can isolate outlying points in both our point clouds to accomplish the matching between source/target clouds.

The input of the RANSAC algorithm determines how many points need to be selected (user controlled). The output of the algorithm determines which specific points need to be selected (are inliers to the data-set – algorithm controlled).

  • How does the Euclidean distance help in better aligning the point clouds?

The intention is to keep minimizing the euclidean distance between all the points (or a subset) of points selected in the point clouds. This is done iteratively. For example, this might be the outcome of the first ICP iteration:

Figure 8: After one ICP iteration

The top peak of the source cloud almost coincides with the target cloud – thus that point has a very small euclidean distance. But what about the rest? – they still have large euclidean distances. Thus, the algorithm will iterate again to minimize the euclidean distance of the other points as well. This might be a second outcome of the ICP iteration:

Figure 9: After a second ICP iteration

In Figure 9, what occurred was the source cloud was rotated. This has a significant impact on the euclidean distances of the other points. Although the peak points have moved away from the target cloud and have a higher euclidean distance (compared to Figure 8), the three points at the lower end of the point clouds have a more minimized euclidean distance between the points.

Figure 10: After a third ICP iteration

As can be seen in Figure 10, the next iteration with a simple translation further reduced the euclidean distance among the sub-set of chosen points and finally another step (more translation) might optimistically lead to alignment with a very small euclidean distance between the selected sub-set of points.

Each ICP Iteration entails the following steps:

  1. Find closest points: using the Euclidean distance between the target/source point clouds.
  2. Calculate alignment: calculate what is the best rotation/translation required to be performed on the source point cloud to align it with the target point cloud.
  3. Regenerate source cloud: after applying the transformation (rotation and/or translation) to the source cloud, regenerate the modified point cloud. The visual effect of this is the source cloud either appears closer or further from the target cloud.

When implementing ICP, after each iteration (to align 3D – x,y,z point clouds) you end up with a 4×4 transformation matrix. This summarizes what the algorithm thinks is the best translation and/or rotation of the source cloud to align with the target cloud. The transformation matrix is composed of an inner 3×3 rotation matrix and the 4th column is the translation vector (3×1).

Transformation Matrix = \begin{bmatrix} R1 & R2 & R3 & x \\ R4 & R5 & R6 & y\\ R7 & R8 & R9 & z\\ 0 & 0 & 0 & 1 \end{bmatrix}

The translation vector seems straight-forward but the rotation matrix to me seemed a little confusing. This is because the rotation matrix was written in a matrix form, but quite simply we can understand the process if we understand how the 3×3 matrix is formed:

We know that we can rotate with respect to x and/or y and/or z. Thus, we would have three angles with respect to each of these axis.

% Rotation values with respect to each axis (In Degrees):
R_{\text{x}} = 5.73 degrees;
R_{\text{y}} = 11.46 degrees;
R_{\text{z}} = 17.19 degrees;

But, 1 radian = 57.2957795 degree, thus if we converted the above to radians:

% Rotation values with respect to each axis (In Radians):
R_{\text{x}} = 0.1;
R_{\text{y}} = 0.2;
R_{\text{z}} = 0.3;

Now to decompose these angles into matrix form:

R_{\text{x}} = \begin{bmatrix} 1 & 0 & 0\\ 0 & cos(R_{\text{x}}) & -sin(R_{\text{x}})\\ 0 & sin(R_{\text{x}}) & cos(R_{\text{x}}) \end{bmatrix}

R_{\text{y}} = \begin{bmatrix} cos(R_{\text{y}}) & 0 & sin(R_{\text{y}})\\ 0 & 1 & 0\\ -sin(R_{\text{y}}) & 0 & cos(R_{\text{y}}) \end{bmatrix}

R_{\text{z}} = \begin{bmatrix} cos(R_{\text{z}}) & -sin(R_{\text{z}}) & 0\\ sin(R_{\text{z}}) & cos(R_{\text{z}}) & 0\\ 0 & 0 & 1 \end{bmatrix}

And finally, to combine the above individual R_{\text{x}}, R_{\text{y}} and R_{\text{z}} matrices into a single 3×3 Rotation Matrix that defines the rotation in all the three axis is obtained by:

Rotation Matrix = R_{\text{x}} * R_{\text{y}} * R_{\text{z}}

And to get the entire 4×4 Transformation Matrix we just add the rotation and translation vectors:

TransformationMatrix_{\text{4x4}}  = RotationMatrix_{\text{3x3}} + TranslationVector_{\text{3x1}} and we add [0, 0, 0, 1] as the last row to the Transformation matrix every time to yield homogeneous matrix operations.

I’d now like to end this post by answering three basic questions:

  1. What is ICP: Quite simply, in this post ICP allows us to register two sets of points clouds: a source cloud to a target cloud by minimizing the point-to-point squared euclidean distance.
  2. Why ICP: Image registration (matching one point cloud with another) has numerous applications in creating 3D maps of geographical places and a more obvious one that comes to my mind is combining different biomedical imaging modalities. An example of the latter might be an imaging modality that gives functional/physiological 3D snap-shots of the region being scanned (little anatomical information) whereas another image modality would provide a lot more 3D anatomical detail of the region being scanned but no functional data, we can use image registration techniques to combine both images and get a combined image that has extremely fine anatomically detail as well as functional information which wouldn’t have been obtained using an single imaging modality.
  3. How to perform ICP: There are numerous open-source libraries available that perform the algorithmic steps of ICP. All you need is 2 data-sets: your target cloud and the source cloud. Personally having used the open-source Point Cloud Library – PCL, I would highly recommend it as a easy starting point for quick implementation. This tutorial explains how to use ICP with the PCL infrastructure, and equipped with the background from this post you will see a reasonably deep understanding of what is happening behind the scenes.



Leave a Reply

Fill in your details below or click an icon to log in: Logo

You are commenting using your account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s

Virtual Fashion Technology

Virtual Fashion Education


"chúng tôi chỉ là tôi tớ của anh em, vì Đức Kitô" (2Cr 4,5b)


News About Tech, Money and Innovation


Modern art using the GPU

Theme Showcase

Find the perfect theme for your blog.


Learn to Learn

Gocomay's Blog

Con tằm đến thác vẫn còn vương tơ

Toán cho Vật lý

Khoa Vật lý, Đại học Sư phạm Tp.HCM - ĐT :(08)-38352020 - 109

Maths 4 Physics & more...

Blog Toán Cao Cấp (M4Ps)

Bucket List Publications

Indulge- Travel, Adventure, & New Experiences


‎"Behind every stack of books there is a flood of knowledge."

The Blog

The latest news on and the WordPress community.

%d bloggers like this: