ISPRS Commission III, Vol.34, Part 3A ,Photogrammetric Computer Vision“, Graz, 2002 
  
There are two major restrictions of the ICP algorithm. 
First, it is implicitly assumed that one of the data sets is 
a subset of the other. The presence of points that have no 
corresponding point in the other set leads to incorrect as- 
signments. Several different approaches to threshold out- 
liers have been presented, see the literature cited in (Eggert 
et al., 1998). Secondly, the ICP algorithm is a two set ap- 
proach and is not directly extendable to multiple data sets. 
It is not sufficient to apply registration to consecutive pairs 
of 3D point sets, since alignment errors accumulate and 
certain point sets will be very poorly adjusted. There have 
been several approaches to the simultaneous registration of 
all data sets, see e.g. the spring force model of (Eggert et 
al., 1998). (Bergevin et al., 1996) apply incremental trans- 
formations to all the moving systems with respect to a fixed 
system and use point-to-tangent plane correspondences in 
the overlapping regions. 
2.1 The ICP algorithm 
A point set (‘data’ shape) is rigidly moved (registered, po- 
sitioned) to be in best alignment with the corresponding 
CAD model (‘model’ shape) in the following iterative way. 
In the first step of each iteration, for each point of the data 
shape, the closest point in the model shape is computed. 
This is the most time consuming part of the algorithm and 
can be implemented efficiently, e.g. by using an octree data 
structure. As result of this first step one obtains a point 
sequence Y — (y,, y»,...) of closest model shape points 
to the data point sequence X = (x1, X2,...). Each point 
X; corresponds to the point y; with the same index. 
In the second step of each iteration the rigid motion M is 
computed such that the moved data points M (x;) are clos- 
est to their corresponding points y;, where the objective 
function to be minimized is 
Y ly, - MG. 
This least squares problem can be solved explicitly, see e.g. 
(Besl McKay, 1992, Horn, 1987). The translational part of 
M brings the center of mass of X to the center of mass of 
Y. The rotational part of M can be obtained as the unit 
eigenvector that corresponds to the maximum eigenvalue 
of a symmetric 4 x 4 matrix. The solution eigenvector is 
nothing but the unit quaternion description of the rotational 
part of M. 
After this second step the positions of the data points are 
updated via Xnew = M (Xoia). Now step 1 and step 2 are 
repeated, always using the updated data points, as long as 
the change in the mean-square error falls below a preset 
threshold. The ICP algorithm always converges monoton- 
ically to a local minimum, since the value of the objective 
function is decreasing both in steps 1 and 2. 
3 KINEMATICS 
An important part of the algorithm uses instantaneous 
kinematics. Thus, we briefly describe the most important 
facts before going into the details of the algorithm. 
A - 266 
Consider a continuous one-parameter motion of a rigid 
body in space. If x is a point in Euclidean three-space, 
the symbol v(x) denotes the velocity vector of that point 
of the moving body which is at this moment at position x. 
Thus v(x) is a time-dependent vector attached to the point 
x. It is well known that at some instant t, a smooth motion 
has a velocity vector field of the form 
v(x)=€+exx, (1) 
with vectors c, € € R®. Thus the velocity vector field (or 
the infinitesimal motion) at some instant ¢ is uniquely de- 
termined by the pair (¢, €). 
Of special interest are the uniform motions, whose veloc- 
ity vector field is constant over time. It is well known 
that apart from the trivial uniform motion, where nothing 
moves at all and all velocities are zero, there are the fol- 
lowing three cases: 
1. Uniform translations have ¢ = o, but € Z o, i.e, 
all velocity vectors equal c. The paths of a uniform 
translation are straight lines parallel to c. 
2. Uniform rotations with nonzero angular velocity 
about a fixed axis. We have € - € = 0, but c Z o. 
The point trajectories of a uniform rotation are circles 
with the rotational axis as common axis. 
  
Figure 1: Helical motion. 
3. Uniform helical motions are the superposition of a 
uniform rotation and a uniform translation parallel to 
the rotation’s axis. They are characterized by c-€ # 0. 
If the point x is situated on the axis, its path coincides 
with the axis. The trajectories of the other points are 
helices. 
If w is the angular velocity of the rotation, and v the 
velocity of the translation, then p = v/w is called the 
pitch of the helical motion. We use the convention 
that w is nonnegative, that p > 0 for right-handed 
helical motions, and that p < 0 for left-handed ones. 
To further clarify the concept of a uniform helical mo- 
tion, we assume the x3-axis of a Cartesian system 
(71,2, x3) to be the helical axis (see Fig. 1). A uni- 
form helical motion may then be written as x + x(t) 
with 
cost —sint 0 0 
x) == sind. «cost «oil x4 0 5 (2) 
0 0 «1 pt 
p — 0 means a uniform rotation, and for p — oo the 
motion tends to a uniform translation.