Scan 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 localization of the robot with the 3D scanner were precise, 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. Furthermore, Robot motion on natural surfaces has to cope with yaw, pitch and roll angles, turning pose estimation into a problem in six mathematical dimensions. A fast variant of the ICP algorithm registers the 3D scans in a common coordinate system and relocalizes the robot. The basic algorithm was invented in 1992 and can be found, e.g., in [3].

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 [3]. However, this theorem does not hold in our case, since we use a maximum tolerable distance $d_$max for associating the scan data. Here $d_$max is set to 15 cm for the first 15 iterations and then this threshold is lowered to 5 cm. Fig. (left) shows two 3D scans aligned only according to the error-prone odometry-based pose estimation. The point pairs are marked by a line.

Figure: Point pairs for the ICP scan matching algorithm. The left image show parts of two 3D scans and the closest point pairs as black lines. The right images show the point pairs in case of semantically based matching (top) whereas the bottom part shows the distribution with closest points without taking the semantic point type into account.