A new approach for solving the Five-Point Relative Pose Problem for vision-based estimation and control

The problem of finding the relative camera pose between two calibrated camera views given five matched feature points is called the Five Point Relative Pose Problem. There exists a variety of solutions in the literature. However, existing methods rely on solving an optimization problem that is based on the Essential matrix. The Essential matrix has fundamental weaknesses, and introduces these weaknesses into algorithms that employ it. In this paper, we propose a new and practical method eschews the essential matrix by representing the pose estimation problem in the quaternion space. The new method has numerous advantages. Unlike the Essential Matrix, it is not prone to problems when faced with coplanar points or zero translation between two camera views. Rotation, scaled translation, and scaled depth of the points with respect to both camera frames are simultaneously recovered. Furthermore, the algorithm is robust to noise and can be easily extended to more than five points. Investigations using simulated images under noise have validated the new method and verify that the algorithm can be used in practical context such as Position Based Visual Servoing.