You are on page 1of 11

# Recently, I’ve had the opportunity to learn and deepen my understanding of the Iterative Closest Point (ICP) algorithm

. This is probably one of the first algorithms that I’ve actually not just read about but tried to implement to a specific problem. 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 scanners, time-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.

Y. the source is now almost completely aligned to the target cloud in terms of rotation (but not translation).   Next. after niterations the source cloud seems to be aligned slightly better in terms of rotation (getting there). It does so iteratively as depicted above. ICP allows us to converge a source point cloud having 6 degrees of freedom (DoF) from the target point cloud. The steps are roughly described below:  Initially the source cloud is completely mis-aligned with respect to the target cloud. Figure 2: Source cloud converged on Model cloud after ICP ICP is a registration technique that uses geometry information (X. Translation about the x-axis . What are the 6 degrees of freedom? 1.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. Z) and not intensity/color to register the source point cloud to the target point cloud. Finally after even more (n + k + t) iterations the source cloud completely overlaps the target cloud and is aligned in both translation and rotation. after a few more iterations (n + k).

Rotation about the y-axis (also called yaw) 6. Since we are talking about ICP with respect to point clouds. Translation about the y-axis 3. 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 . 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.2. Translation about the z-axis 4. Rotation about the x-axis (also called pitch) 5. 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.

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. 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. we get a theoretically perfect alignment: Figure 6: Perfect transformation match between target and source clouds Now that we know a little about correspondences. it simply means the point on the target cloud which has the smallestEuclidean distance from a point on the source cloud. How do you define closest? Well.The first step in ICP matching between the target cloud to the source cloud is to determine correspondences between the two clouds. the Euclidean distance would be the line segment connecting point p and q. Figure 5: Correspondences between target and source clouds And if we use the transformation of merely those selected points. The explanation of Euclidean distance is if we had two points p (on the target cloud) and q (on the source cloud). 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. The .

the points selected for the matching are shown below (in green).wikipedia. 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. a rotation in 3D space can be . we are aware that the source cloud needs to be rotated and/or translated to be correctly aligned with the target cloud.z) and Q (x. we would like to transform the source cloud relative to the target cloud. As mentioned above. Thus.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 http://en. such that the sum of all point-wise Euclidean distances is minimized. ICP now becomes a minimization problem.A is a function of a rotation and a translation. in our point cloud (3D) case we have P (x. For point P and Q. All we need to do is define what has to be minimized. at-least for these example point clouds. From above.org/wiki/Euclidean_distance#equation_1 Thus. Thus we conclude that the transformation represented by a matrix multiplication of a 3D point with the rotation matrix . Using matrix algebra.y.distance can be quantitatively calculated by the below formula (from Wikipedia). 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.

In other words. and a translation points in the subset of points chosen to be used in each ICP points are chosen from each of the source cloud and the target represent an represent its corresponding point in the target cloud which are assumed to be in correspondence with each other.translation can be represented by the vector addition of a 3D point vector . square it and do that for all the other points in the sub-set and take the mean. We represent this transformation as Now. squared error function that is based on rotation and translation. let there be iteration. We said that it is the source cloud that has to be transformed. This is what ICP is minimizing! . To fix notation. This gives us a mean. if we want to compute the Euclidean distance between the target point and the source cloud point after transforming the source point: For the entire point cloud. take the Euclidean distance. Let arbitrary point in the source cloud and cloud. This forms the that we wish to minimize All we are saying is that for each point from the target cloud-source cloud pair we consider. we can compute the total error in the cloud alignment using a transformation error function by summing over all the points in the subset considered.

For example. The output of the algorithm determines which specific points need to be selected (are inliers to the data-set – algorithm controlled). Thus. we need to understand Random Sample Concensus (RANSAC). But what about the rest? – they still have large euclidean distances. The input of the RANSAC algorithm determines how many points need to be selected (user controlled). This might be a second outcome of the ICP iteration: .  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. the algorithm will iterate again to minimize the euclidean distance of the other points as well. This is done iteratively. 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.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. 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.

Find closest points: using the Euclidean distance between the target/source point clouds. what occurred was the source cloud was rotated. This has a significant impact on the euclidean distances of the other points.Figure 9: After a second ICP iteration In Figure 9. Figure 10: After a third ICP iteration As can be seen in Figure 10. Each ICP Iteration entails the following steps: 1. . 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. 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.

The visual effect of this is the source cloud either appears closer or further from the target cloud. after each iteration (to align 3D – x. The transformation matrix is composed of an inner 3×3 rotation matrix and the 4th column is the translation vector (3×1).2957795 degree. 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. % Rotation values with respect to each axis (In Degrees): = 5.46 degrees.y. 1 radian = 57.z point clouds) you end up with a 4×4 transformation matrix. But. we would have three angles with respect to each of these axis. . regenerate the modified point cloud. This is because the rotation matrix was written in a matrix form.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. = 17.1.73 degrees.19 degrees. This summarizes what the algorithm thinks is the best translation and/or rotation of the source cloud to align with the target cloud. Transformation Matrix = The translation vector seems straight-forward but the rotation matrix to me seemed a little confusing. Regenerate source cloud: after applying the transformation (rotation and/or translation) to the source cloud. = 11. When implementing ICP. Thus. thus if we converted the above to radians: % Rotation values with respect to each axis (In Radians): = 0. 3.

to combine the above individual matrices into a single 3×3 Rotation Matrix that defines the rotation in all the three axis is obtained by: Rotation Matrix = And to get the entire 4×4 Transformation Matrix we just add the rotation and translation vectors: = we add as the last row to the Transformation matrix every time to yield homogeneous matrix operations. 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. and .3. I’d now like to end this post by answering three basic questions: 1. = 0. What is ICP: Quite simply. Now to decompose these angles into matrix form: = = = And finally.= 0.

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. . 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. This tutorial explains how to use ICP with the PCL infrastructure. All you need is 2 data-sets: your target cloud and the source cloud.2. I would highly recommend it as a easy starting point for quick implementation. 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. 3. and equipped with the background from this post you will see a reasonably deep understanding of what is happening behind the scenes. How to perform ICP: There are numerous open-source libraries available that perform the algorithmic steps of ICP. Personally having used the open-source Point Cloud Library – PCL.