Reliable Camera-Based Positioning Robust to Initial Pose Error

In GNSS degraded environments, cameras have great potential to be used as navigation sensors in various applications such as UAV landing, autonomous driving, indoor navigation, etc. Using a set of visible features with known locations on the map, the 6 degrees of freedom (DOF) pose (position and attitude) of the camera can be estimated. This is known as the Perspective-n-Point (PnP) problem. However, the performance of pose estimation is very sensitive to the initial rough estimate of the camera pose, as the measurement equation is highly non-linear. This problem degrades the performance of visual navigation and has limited its use in applications that require high reliability. In this work, we propose an innovative algorithm based on Feasible Pursuit Point and Successive Convex Approximation (FPP-SCA) method by Mehanna et al. (2015) to solve the PnP problem. The algorithm exhibits a significant global convergence property so that the camera pose can be accurately estimated even when the initial position error is large. The overall performance of the proposed method is shown to outperform state-of-the-art approaches and to be more resistant to incorrect initial conditions and measurement noise levels from simulations and experiments in the context of UAV landing.