Range Image Registration

We use the well-known Iterative Closest Points (ICP) algorithm to calculate a rough approximation of the transformation while the robot is acquiring the 3D scans. The ICP algorithm calculates iteratively the point correspondence. In each iteration step, the algorithm selects the closest points as correspondences and calculates the transformation ( $\M R, \V t$) for minimizing the equation

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

where $N_m$ and $N_d$, are the number of points in the model set $M$ or data set $D$, respectively and $w_{ji}$ are the weights for a point match. The weights are assigned as follows: $w_{ji} = 1$, if $\V m_i$ is the closest point to $\V d_j$ within a close limit, $w_{ji} = 0$ otherwise. It is shown that the iteration terminates in a minimum (5). The assumption is that in the last iteration step the point correspondences are correct. In each iteration, the transformation is calculated by the quaternion based method of Horn (14).