next up previous
Next: Surface Extraction Up: The Context: Outdoor SPLAM Previous: Item 2: Heuristic Initial

Item 3: ICP for 3D Scan Matching.

The 3D scans have to be merged into one coordinate system. This process is called registration. 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:

$\displaystyle 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

$\displaystyle E(\M R, \V t)$ $\displaystyle \propto$ $\displaystyle \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) [14]. In earlier work [19,24,25] 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.,

$\displaystyle \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)

$\displaystyle M' = $     (4)
$\displaystyle end{tex2html_deferred}{ \V m'_{i} = \V m_{i} - \V c_{m} $     (5)
$\displaystyle end{tex2html_deferred}}_{1,\ldots,N}, %\label{d_neu1}
D' = $     (6)
$\displaystyle end{tex2html_deferred}{ \V d'_{i}$     (7)
$\displaystyle end{tex2html_deferred}= \V d_{i}$     (8)
$\displaystyle end{tex2html_deferred}, - \V c_{d} $     (9)
$\displaystyle end{tex2html_deferred}}_{1,\ldots,N}.$     (10)

After substituting (3) and (4) into the error function, $ E(\M R, \V t)$ eq. (2) becomes:
$\displaystyle E(\M R, \V t) \propto
\sum_{i=1}^{N}\norm {\V m'_{i}-\M R \V d'_i}^2$   with$\displaystyle \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

\begin{displaymath}\M H = \sum_{i=1}^{N} \V d'_i \V m'^T_i
= \left(
...} & S_{yz} \\
S_{zx} & S_{zy} & S_{zz} \\
\end{array}\right),\end{displaymath}     (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 have proposed and evaluated algorithms to accelerate ICP, namely point reduction and approximate $ k$d-trees [19,24,25]. They are used here, too.

next up previous
Next: Surface Extraction Up: The Context: Outdoor SPLAM Previous: Item 2: Heuristic Initial
root 2006-03-16