A Novel Closed-Form Approach for Enhancing Efficiency in Pose Estimation From 3D Correspondences

Determining the pose using 3D data captured from two distinct frames holds significant significance in various robotic applications such as odometry, simultaneous localization and mapping and place recognition. Typically, this pose is calculated through the resolution of a least-squares problem, which involves establishing correspondences between points, points and planes, or points and lines. This non-linear least-squares problem can be addressed either through iterative optimization or, with greater efficiency, through closed-form solutions achieved via polynomial system solvers. In this letter, the focus is on enhancing the computational efficiency of polynomial solvers utilizing resultants. The newly proposed methodology outperforms previous techniques in terms of speed, all while maintaining identical levels of accuracy and robustness. Through simulations and real-world experiments, we validate the superior precision and robustness of the proposed algorithm when compared with prior methods.