Matching as an Optimization Problem

The following method for registration of point sets is part of many publications, so only a short summary is given here. The complete algorithm was invented in 1991 and can be found, e.g., in [4,6,22]. The method is called Iterative Closest Points (ICP) algorithm.

Given two independently acquired sets of 3D points, $M$ (model set, $\vert M\vert = N_m$) and $D$ (data set, $\vert D\vert = N_d$) which correspond to a single shape, we want to find the transformation consisting of a rotation $\MR$ and a translation $\Vt$ which minimizes the following cost function:

$$E(\MR , \Vt ) = \sum_{i=1}^{N_m}\sum_{j=1}^{N_d}w_{i,j}\norm {\Vm _{i}-(\MR\Vd _j+\Vt )}^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 $\MR$ and $\Vt$ that minimizes $E(\MR , \Vt )$ on the base of the corresponding points.

The ICP algorithm calculates iteratively the point correspondence. In each iteration step, the algorithm selects the closest points as correspondences and calculates the transformation ($\MR , \Vt$) for minimizing equation (1). It is shown that the iteration terminates in a minimum [4]. The assumption is that in the last iteration step the point correspondences are correct.

In each iteration, the transformation is calculated by the quaternion based method of Horn [12]. A unit quaternion is a 4 vector $\dot q = (q_0,q_x,q_y,_z)^T$, where $q_0 \geq 0, q_0^2+q_x^2+q_y^2+q_z^2 = 1$. It describes a rotation axis and an angle to rotate around that axis. A $3 \times 3$ rotation matrix $\MR$ is calculated from the unit quaternion according the the following scheme:

$$\MR = \left( \begin{array}{ccc} (q_0^2 + q_x^2 - q_y^2 - q_z^... ...+ q_xq_0) & (q_0^2 - q_x^2 - q_y^2 + q_z^2) \end{array}\right).$$

To determine the transformation, the mean values (centroid vectors) $\Vc _m$ and $\Vc _d$ are subtracted from all points in $M$ and $D$, respectively, resulting in the sets $M'$ and $D'$. The rotation expressed as quaternion that minimizes equation (1) is the largest eigenvalue of the cross-covariance matrix

$$\begin{scriptsize} \MN = \left( \begin{array}{cccc} (S_{xx} +... ... S_{xx} - S_{yy} + S_{zz}) \end{array}\right), \end{scriptsize}$$

with $S_{\alpha\beta} := \sum_{i=1}^{N_m}\sum_{j=1}^{N_d} w_{i,j} m'_{i\alpha} d'_{j\beta}$. After the calculation of the rotation $\MR$, the translation is $\Vt = \Vc _m - \MR\Vc _d$ [12]. Figure 4 shows three steps of the ICP algorithm.

The time complexity of the algorithm mainly depends on determination of the closest points (brute force search O($n^2$) for 3D scans of $n$ points). Several enhancements have been proposed [4,17]. We have implemented $k$d-trees as proposed by Simon et al., combined with the above-described reduced points. Table 1 summarizes the results of different experiments on a Pentium-III-800. The starting point for optimization is given by the robot odometry.

Table 1: computing time of the different 3D scan matching implementations for two scans of the GMD Robobench (figure 4). The number of all points is 46336 (181 $\times$ 256) and the number of the reduced points is 4910.

points used	time	# iter.
all points & brute force	3 hours 47 min	27
reduced points & brute force	3 min 6 sec	25
all points & $k$D-tree	6 sec	27
reduced points & $k$D-tree	$<$1.4 sec	25

Figure 4: Registration of two 3D-scans of an office environment with the ICP algorithm. Top left: Initial alignment based on odometry. Top right: Alignment after 2 iterations. Bottom left: after 5 iterations. Bottom right: Final alignment after 25 iterations. The scans correspond to figure 2. The number of sampled 3D points is 46336 per 3D scan and the number of reduced points is 4910.