Robust and accurate pose estimation for vision-based localisation

Camera pose estimation (perspective-n-points, or PnP) is a well-studied problem in computer vision with many applications in robotics. However state-of-art approaches often consider the task of pose estimation with respect to an object where the ratio between the furthest and closest point is small. Localisation in the context of simultaneous localisation and mapping (SLAM) violates this constraint and the naive application of PnP algorithms can lead to biased and imprecise estimates. In this article, we explore weighted object-space errors to provide an efficient, accurate and robust pose estimation solution. State-of-the-art results are shown in realistic scenarios and a practical vision framework is proposed that can be used for visual SLAM. An implementation of the proposed algorithm has been made available as open-source software in Matlab and C++.