In this paper, we present a method for real-time pose estimation of rigid objects in heavily cluttered environments. At its core, the method relies on the template matching method proposed by Hinterstoisser et al., which is used to generate pose hypotheses. We improved the method by introducing a compensation for bias toward simple shapes and by changing the way modalities such as edges and surface normals are combined. Additionally, we incorporated surface normals obtained with photometric stereo that can produce a dense normal field at a very high level of detail. An iterative algorithm was employed to select the best pose hypotheses among the possible candidates provided by template matching. An evaluation of the pose estimation reliability and a comparison with the current state-of-the-art was performed on several synthetic and several real datasets. The results indicate that the proposed improvements to the similarity measure and the incorporation of surface normals obtained with photometric stereo significantly improve the pose estimation reliability. 
Real-time pose estimation of rigid objects in heavily cluttered environments