The ICP Algorithm

The ICP Algorithm was developed by Besl and McKay [5] and is usually used to register two given point sets in a common coordinate system. The algorithm calculates iteratively the registration. In each iteration step, the algorithm selects the closest points as correspondences and calculates the transformation, i.e., rotation and translation ( $\mathbf{R}, \mathbf{t}$), for minimizing the equation

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

where $N_m$ and $N_d$, are the number of points in the model set $M$ and data set $D$, 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$, $w_{ji} = 0$ otherwise. Eq. (1) can be reduced to

$$E(\mathbf{R}, \mathbf{t}) \propto \frac{1}{N} \sum_{i=1}^N \left\lvert\left\lvert \mathbf{m}_i - (\mathbf{R} \mathbf{d}_i + \mathbf{t}) \right\rvert\right\rvert ^2,$$ (2)

with $N = \sum_{i=1}^{N_m}\sum_{j=1}^{N_d}w_{i,j}$, since the correspondence matrix can be represented by a vector $\mathbf{v}$ containing the point pairs, i.e., $\mathbf{v} = ((\mathbf{d}_1, \mathbf{m}_{f(\mathbf{d}_1)}), (\mathbf{d}_2, \m... ..._2)}), \linebreak\ldots , (\mathbf{d}_{N_d}, \mathbf{m}_{f(\mathbf{d}_{N_d})}))$, with $f(\mathbf{x})$ the search function returning the closest point. The assumption is that in the last iteration step the point correspondences, thus the vector of point pairs, are correct.

In each ICP iteration, the transformation can be calculated by any of these four methods: A SVD based method of Arun et al. [1], a quaternion method of Horn [12], an algorithm using orthonormal matrices of Horn et al. [13] and a calculation based on dual quaternions of Walker et al. [24]. These algorithms show similar performance and stability concerning noisy data [15].

Besl and McKay show that the iteration terminates in a minimum [5]. Note: Normally, implementations of ICP would use a maximal distance for closest points to handle partially overlapping point sets. In this case the proof in [5] does no longer hold, since the number of points as well as the value of $E(\mathbf{R}, \mathbf{t})$ might increase after applying a transformation.