A high accuracy method for pose estimation based on rotation parameters

In this paper, A high-precision pose estimation method based on rotation parameters is proposed. By introducing the parameterization of rotation matrix, we first formulate a nonlinear least-squares cost function whose optimality conditions constitute a system of three third-order polynomials. Then, based on Gröbner basis method, we solve the solution of polynomial equation directly, and then the attitude is calculated by back-substitution method. More importantly, one of the main advantages of our method is scalability, because the order of the polynomial equations we solve is independent of the number of points involved in the estimation. Finally, we compare the performance of our algorithm with the leading PnP method, both in simulations and experiments, and show that the proposed method achieves the accuracy of approaching the OPnP (optimal solution to the PnP problem).