Item 3: ICP for 3D Scan Matching.

The 3D scans have to be merged into one coordinate system. This process is called registration. 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, $M$ (model set) and $D$ (data set) which correspond to a single shape, we aim to find the transformation consisting of a rotation $\M R$ and a translation $\V t$ which minimizes the following cost function:

$$E(\M R, \V t) = \sum_{i=1}^{\vert M\vert}\sum_{j=1}^{\vert D\vert}w_{i,j}\norm {\V m_{i}-(\M R \V d_j+\V t)}^2.$$ (1)

$w_{i,j}$ is assigned 1 if the $i$-th point of $M$ describes the same point in space as the $j$-th point of $D$. Otherwise $w_{i,j}$ is 0. Two things have to be calculated: First, the corresponding points, and second, the transformation ($\M R$, $\V t$) that minimizes $E(\M R, \V t)$ 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 ( $\M R, \V t$) 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 $d_$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 ($\M R$, $\V t$) has to be computed. Eq. (1) can be reduced to

$$E(\M R, \V t) \propto \frac{1}{N} \sum_{i=1}^N \norm {\V m_i - (\M R \V d_i + \V t)}^2,$$ (2)

with $N = \sum_{i=1}^{\vert M\vert}\sum_{j=1}^{\vert D\vert}w_{i,j}$, since the correspondence matrix can be represented by a vector containing the point pairs.

Four direct methods are known to minimize eq. (2) [14]. In earlier work [19,24,25] 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 $\M R$. The first step of the computation is to decouple the calculation of the rotation $\M R$ from the translation $\V t$ using the centroids of the points belonging to the matching, i.e.,

$$\V c_m = \frac{1}{N} \sum_{i=1}^{N} \V m_{i}, \qquad \qquad \V c_d = \frac{1}{N} \sum_{i=1}^{N} \V d_{j}$$ (3)

and

$$M' =$$ (4)

$$end{tex2html_deferred}{ \V m'_{i} = \V m_{i} - \V c_{m}$$ (5)

$$end{tex2html_deferred}}_{1,\ldots,N}, %\label{d_neu1} \qquad D' =$$ (6)

$$end{tex2html_deferred}{ \V d'_{i}$$ (7)

$$end{tex2html_deferred}= \V d_{i}$$ (8)

$$end{tex2html_deferred}, - \V c_{d}$$ (9)

$$end{tex2html_deferred}}_{1,\ldots,N}.$$ (10)

After substituting (3) and (4) into the error function, $E(\M R, \V t)$ eq. (2) becomes:

$$E(\M R, \V t) \propto \sum_{i=1}^{N}\norm {\V m'_{i}-\M R \V d'_i}^2 \quad \V t = \V c_m - \M R \V c_d.$$ (11)

The registration calculates the optimal rotation by $\M R = \M V \M U^T$. Hereby, the matrices $\M V$ and $\M U$ are derived by the singular value decomposition $\M H = \M U \M \Lambda \M V^T$ of a correlation matrix $\M H$. This $3 \times 3$ matrix $\M H$ is given by

$$\M H = \sum_{i=1}^{N} \V d'_i \V m'^T_i = \left( \begin{array... ...} & S_{yz} \\ S_{zx} & S_{zy} & S_{zz} \\ \end{array}\right),$$ (12)

with $S_{xx} = \sum_{i=1}^{N} \m'_{ix} d'_{ix}, \S_{xy} = \sum_{i=1}^{N} \m'_{ix} d'_{iy}, \\ ldots \,$[2].

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