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 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.

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 1992 and can be found, e.g., in [7]. 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 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}^{N_m}\sum_{j=1}^{N_d}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 minimize $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 (). 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 [7]. 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, given that the 3D scans overlap only partially. Fig. (top) shows three frames, i.e., iteration steps, of the ICP algorithm. The bottom part shows the start poses $(x,z,\theta_y)$ from which a correct matching is possible, here with only three degrees of freedom.

Figure: Top row: Left: Initial odometry based pose of two 3D scans. Middle: Pose after five ICP iterations. Right: final alignment, pairwise matching. Bottom row: The poses are marked in $(x,z,\theta_y)$ from which a correct alignment of two 3D scans is possible.