teorie icp

7/28/2019 Teorie ICP

1/11

Recently, Ive 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 Ive 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 Ive 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, well do the following:

Understand what ICP is

Understand the different steps involved in ICP

What a transformation matrix looks like

Im 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 camerasor using regular cameras and applyingstereo

visiontechniques to the left and right images. However, this isnt the focus of this article. Weassume, 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 targetpoint cloud and the transformed point cloud as thesource point cloud.
http://en.wikipedia.org/wiki/3D_scannerhttp://en.wikipedia.org/wiki/3D_scannerhttp://en.wikipedia.org/wiki/3D_scannerhttp://en.wikipedia.org/wiki/3D_scannerhttp://en.wikipedia.org/wiki/Time-of-flight_camerahttp://en.wikipedia.org/wiki/Time-of-flight_camerahttp://en.wikipedia.org/wiki/Time-of-flight_camerahttp://en.wikipedia.org/wiki/Computer_stereo_visionhttp://en.wikipedia.org/wiki/Computer_stereo_visionhttp://en.wikipedia.org/wiki/Computer_stereo_visionhttp://en.wikipedia.org/wiki/Computer_stereo_visionhttp://www.wisegai.com/wp-content/uploads/2012/11/ICPDrawing1.pnghttp://en.wikipedia.org/wiki/Computer_stereo_visionhttp://en.wikipedia.org/wiki/Computer_stereo_visionhttp://en.wikipedia.org/wiki/Time-of-flight_camerahttp://en.wikipedia.org/wiki/3D_scannerhttp://en.wikipedia.org/wiki/3D_scanner


2/11

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 Modelcloud after ICP

ICP is a registrationtechnique 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,

afterniterations 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
http://www.wisegai.com/wp-content/uploads/2012/11/ICPDrawing21.png


3/11

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 followingimage from Wikipediavisually 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
http://en.wikipedia.org/wiki/Six_degrees_of_freedomhttp://en.wikipedia.org/wiki/Six_degrees_of_freedomhttp://en.wikipedia.org/wiki/Six_degrees_of_freedomhttp://www.wisegai.com/wp-content/uploads/2012/11/ICPDrawing3.pnghttp://en.wikipedia.org/wiki/Six_degrees_of_freedomhttp://www.wisegai.com/wp-content/uploads/2012/11/ICPDrawing3.pnghttp://en.wikipedia.org/wiki/Six_degrees_of_freedomhttp://en.wikipedia.org/wiki/Six_degrees_of_freedom


4/11

The first step in ICP matching between the target cloud to the source cloud is to determine

correspondences 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 thecorrespondence problem.

Ideally if we know all the correspondences between the two clouds, we can merely compute

how those points in the source cloud are transformedfrom 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 distancefrom 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
http://en.wikipedia.org/wiki/Correspondence_problemhttp://en.wikipedia.org/wiki/Correspondence_problemhttp://en.wikipedia.org/wiki/Correspondence_problemhttp://en.wikipedia.org/wiki/Euclidean_distancehttp://en.wikipedia.org/wiki/Euclidean_distancehttp://en.wikipedia.org/wiki/Euclidean_distancehttp://www.wisegai.com/wp-content/uploads/2012/11/ICP_Correspondences_Matched.pnghttp://www.wisegai.com/wp-content/uploads/2012/11/ICP_Correspondences.pnghttp://www.wisegai.com/wp-content/uploads/2012/11/ICP_Correspondences_Matched.pnghttp://www.wisegai.com/wp-content/uploads/2012/11/ICP_Correspondences.pnghttp://en.wikipedia.org/wiki/Euclidean_distancehttp://en.wikipedia.org/wiki/Correspondence_problem


5/11

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

http://en.wikipedia.org/wiki/Euclidean_distance#equation_1

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 pointsbetween 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 isnt 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
http://www.wisegai.com/wp-content/uploads/2012/11/ICP_closestEuclideanPoints.pnghttp://en.wikipedia.org/wiki/Euclidean_distancehttp://www.wisegai.com/wp-content/uploads/2012/11/ICP_closestEuclideanPoints.pnghttp://en.wikipedia.org/wiki/Euclidean_distancehttp://www.wisegai.com/wp-content/uploads/2012/11/ICP_closestEuclideanPoints.pnghttp://en.wikipedia.org/wiki/Euclidean_distancehttp://www.wisegai.com/wp-content/uploads/2012/11/ICP_closestEuclideanPoints.pnghttp://en.wikipedia.org/wiki/Euclidean_distancehttp://www.wisegai.com/wp-content/uploads/2012/11/ICP_closestEuclideanPoints.pnghttp://en.wikipedia.org/wiki/Euclidean_distance


6/11

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!


7/11

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:
http://en.wikipedia.org/wiki/RANSAChttp://en.wikipedia.org/wiki/RANSAChttp://en.wikipedia.org/wiki/RANSAChttp://www.wisegai.com/wp-content/uploads/2012/11/ICP_interation1.pnghttp://www.wisegai.com/wp-content/uploads/2012/11/ICP_onlyclosestEuclideanPoints.pnghttp://www.wisegai.com/wp-content/uploads/2012/11/ICP_interation1.pnghttp://www.wisegai.com/wp-content/uploads/2012/11/ICP_onlyclosestEuclideanPoints.pnghttp://en.wikipedia.org/wiki/RANSAC


8/11

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), thethree 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.
http://www.wisegai.com/wp-content/uploads/2012/11/ICP_iteration3.pnghttp://www.wisegai.com/wp-content/uploads/2012/11/ICP_iteration2.pnghttp://www.wisegai.com/wp-content/uploads/2012/11/ICP_iteration3.pnghttp://www.wisegai.com/wp-content/uploads/2012/11/ICP_iteration2.png


9/11

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 3Dx,y,z point clouds) you end up

with a 44 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 33 rotation matrix and the 4th column is the

translation vector (31).

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 33 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;


10/11

= 0.2;

= 0.3;

Now to decompose these angles into matrix form:

=

=

=

And finally, to combine the above individual matrices into a single 33

Rotation Matrix that defines the rotation in all the three axis is obtained by:

Rotation Matrix =

And to get the entire 44 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.

Id 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.


11/11

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 wouldnt 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 LibraryPCL, I would highly recommend

it as a easy starting point for quick implementation.This tutorialexplains how to use ICP withthe PCL infrastructure, and equipped with the background from this post you will see a

reasonably deep understanding of what is happening behind the scenes.
http://www.pointclouds.org/http://www.pointclouds.org/http://www.pointclouds.org/http://pointclouds.org/documentation/tutorials/iterative_closest_point.phphttp://pointclouds.org/documentation/tutorials/iterative_closest_point.phphttp://pointclouds.org/documentation/tutorials/iterative_closest_point.phphttp://pointclouds.org/documentation/tutorials/iterative_closest_point.phphttp://www.pointclouds.org/

teorie icp

Documents