3D Scan Matching

To create a correct and consistent representation of the environment, the acquired 3D scans have to be merged in one coordinate system. This process is called registration. Due to the robot's sensors, the self-localization is usually erroneous and imprecise, so the geometric structure of overlapping 3D scans has to be considered for registration. The odometry-based robot pose serves as a first estimate and is corrected and updated by the registration process. We use the well-known Iterative Closest Points (ICP) algorithm [2] to compute the transformation, consisting of a rotation $\mathbf{R} \in \mathbbm{R}^{3 \times 3}$ and a translation $\mathbf{t} \in \mathbbm{R}^3$. The ICP algorithm computes this transformation in an iterative fashion. In each iteration the algorithm selects the closest points as correspondences and computes the transformation ( $\mathbf{R}, \mathbf{t}$) for minimizing

$$E(\mathbf{R}, \mathbf{t}) \! = \! \sum_{i=1}^{N_m}\sum_{j=1}^{N_d... ...mathbf{m}_{i}-(\mathbf{R} \mathbf{d}_j+\mathbf{t}) \right \vert \right \vert^2,$$

where $N_m$ and $N_d$ are the number of points in the model set $M$, i.e., first 3D scan, or data set $D$, second 3D scan, respectively, and $w_{ji}$ are the weights for a point match. The weights are assigned as follows: $w_{ji} = 1$, if $\mathbf{m}_i$ is the closest point to $\mathbf{d}_j$ within a close limit, $w_{ji} = 0$ otherwise. It is shown in [2] that the iteration terminates in a minimum. The assumption is that in the last iteration the point correspondences are correct. In each iteration the transformation is computed in a fast closed-form manner by the quaternion-based method of Horn [8]. In addition, point reduction and $k$D.trees speed up the computation of the point pairs, such that only the time required for scan matching is reduced to roughly one second [17]. Figure 3 shows three iteration steps for 3D scan alignment.

Figure 3: Three iteration steps of scan alignment process for the two 3D scans presented in Figure 1 (bottom, middle, right).