"Behind every stack of books there is a flood of knowledge."
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:
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.
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:
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:
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?
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:
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.
And if we use the transformation of merely those selected points, we get a theoretically perfect alignment:
Now that we know a little about correspondences, a few questions become obvious:
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
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:
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 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 with the rotation matrix . A translation can be represented by the vector addition of a 3D point and a translation vector .
To fix notation, let there be points in the subset of points chosen to be used in each ICP iteration. In other words, points are chosen from each of the source cloud and the target cloud which are assumed to be in correspondence with each other. Let represent an arbitrary point in the source cloud and 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
Now, 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, we can compute the total error in the cloud alignment using a transformation by summing over all the points in the subset considered. This forms the error function that we wish to minimize
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!
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).
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:
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.
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:
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 =
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): = 5.73 degrees; = 11.46 degrees; = 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): = 0.1; = 0.2; = 0.3;
Now to decompose these angles into matrix form:
And finally, 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:
= and we add 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:
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
Find the perfect theme for your blog.
Learn to Learn
Con tằm đến thác vẫn còn vương tơ
Khoa Vật lý, Đại học Sư phạm Tp.HCM - ĐT :(08)-38352020 - 109
Blog Toán Cao Cấp (M4Ps)
Indulge- Travel, Adventure, & New Experiences
"Behind every stack of books there is a flood of knowledge."
The latest news on WordPress.com and the WordPress community.