Matching as Optimization

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 [5]. 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 want 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 a local minimum of equation (1). In each iteration step, the algorithm selects the closest points as correspondences $w_{i,j}$ and calculates the transformation ($\M R, \V t$) for minimizing equation (1). Fig. 2 shows three steps of the ICP algorithm. Besl and McKay prove that the method terminates in a minimum [5]. The assumption is that in the last iteration step the point correspondences are correct.

In each ICP iteration, the transformation is calculated by the quaternion based method of Horn [15]: A unit quaternion is a 4 vector $\dot q = (q_0,q_x,q_y,q_z)^T$, where $q_0^2+q_x^2+q_y^2+q_z^2 = 1, q_0 \geq 0$. It describes a rotation axis and an angle to rotate around that axis. A $3 \times 3$ rotation matrix $\M R$ is calculated from the unit quaternion according the the following scheme: $\M R =$

$$\begin{eqnarray*} \left( \begin{array}{ccc} (q_0^2 + q_x^2 - q_y^2 - q_z^2) ... ..._xq_0) & (q_0^2 - q_x^2 - q_y^2 + q_z^2) \end{array} \right). \end{eqnarray*}$$

To determine the transformation, the mean values of the paired points (centroid vectors) $\V c_m$ and $\V c_d$ are subtracted from all points in $M$ and $D$, respectively, resulting in the sets $M'$ and $D'$. The rotation expressed as quaternion that minimizes equation (1) is the largest eigenvalue of the cross-covariance matrix

$$\begin{eqnarray*} \M N & = & \left( \begin{array}{cc} (S_{xx} + S_{yy} + S_... ... S_{xz}) & (- S_{xx} - S_{yy} + S_{zz}) \end{array} \right), \end{eqnarray*}$$

with $S_{xx} = \sum_{i=1}^{N_m}\sum_{j=1}^{N_d} w_{i,j} \ m'_{ix} d'_{jx}, \S_{xy} = \sum_{i=1}^{N_m}\sum_{j=1}^{N_d} w_{i,j} \m'_{ix} d'_{jy}$, ...After calculation the rotation $\M R$, the translation is determined by $\V t = \V c_m - \M R \V c_d$ [15]. Fig. 2 shows two 3D scans in their initial, i.e., odometry-based pose, after 5 iterations, and the final pose. 40 iterations are needed to align these two 3D scans correctly.

Figure 2: Left: Initial odometry based pose of two 3D scans. Middle: Pose after five ICP iterations. Right: final alignment.