Range Image Registration and Robot Relocalization

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, $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) [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 $\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 proposed and evaluated algorithms to accelerate ICP, namely point reduction and approximate $k$d-trees [19,23,24]. They are used here, too.