After the 3D data (set ) that contain the object is
found, a given 3D model from the object database is matched into
the point cloud. The model is also saved as 3D point
cloud in the database. The well known iterative closest points
algorithm (ICP) is used to find a matching [4]. The
ICP algorithm calculates iteratively the point
correspondences. In each iteration step, the algorithm selects
the closest points as correspondences and calculates the
transformation, i.e., rotation and translation (
) for
minimizing the equation

where and , are the number of points in the model set or data set , respectively, and are the weights for a point match. The weights are assigned as follows: , if is the closest point to within a close limit, otherwise.

It is shown that the iteration terminates in a minimum [4]. In each iteration, the transformation is calculated by the quaternion based method of Horn [9]. The assumption is that the point correspondences are correct in the last iteration step. Finally, the pose of the model corresponds to the one in the data set.