Multiple 3D scans are necessary to digitalize environments without occlusions. To create a correct and consistent model, the scans have to be merged into one coordinate system. This process is called registration. If the robot carrying the 3D scanner were precisely localized, the registration could be done directly based on the robot pose. However, due to the unprecise robot sensors, self localization is erroneous, so the geometric structure of overlapping 3D scans has to be considered for registration. As a by-product, successful registration of 3D scans relocalizes the robot in 6D, by providing the transformation to be applied to the robot pose estimation at the recent scan point.

The following method registers point sets in a common coordinate
system. It is called *Iterative Closest Points (ICP)*
algorithm [4]. Given two independently acquired sets
of 3D points, (model set) and (data set) which correspond
to a single shape, we aim to find the transformation consisting
of a rotation and a translation which minimizes the
following cost function:

is assigned 1 if the -th point of describes the same point in space as the -th point of . Otherwise is 0. Two things have to be calculated: First, the corresponding points, and second, the transformation (, ) that minimizes on the base of the corresponding points.

The ICP algorithm calculates iteratively the point correspondences. In each iteration step, the algorithm selects the closest points as correspondences and calculates the transformation ( ) for minimizing equation (1). The assumption is that in the last iteration step the point correspondences are correct. Besl et al. prove that the method terminates in a minimum [4]. However, this theorem does not hold in our case, since we use a maximum tolerable distance max for associating the scan data. Such a threshold is required though, given that 3D scans overlap only partially.

In every iteration, the optimal transformation (, ) has to be computed. Eq. (1) can be reduced to

with , since the correspondence matrix can be represented by a vector containing the point pairs.

Four direct methods are known to minimize eq. (2) [17]. In earlier work [19,23,24] we used a quaternion based method [4], but the following one, based on singular value decomposition (SVD), is robust and easy to implement, thus we give a brief overview of the SVD-based algorithm. It was first published by Arun, Huang and Blostein [2]. The difficulty of this minimization problem is to enforce the orthonormality of the matrix . The first step of the computation is to decouple the calculation of the rotation from the translation using the centroids of the points belonging to the matching, i.e.,

and

After substituting (3) and (4) into the error function, eq. (2) becomes:

The registration calculates the optimal rotation by . Hereby, the matrices and are derived by the singular value decomposition of a correlation matrix . This matrix is given by

with [2].

We proposed and evaluated algorithms to accelerate ICP, namely point reduction and approximate d-trees [19,23,24]. They are used here, too.