Do not Omit Local Minimizer: a Complete Solution for Pose Estimation from 3D Correspondences

Estimating pose from given 3D correspondences, including point-to-point, point-to-line and point-to-plane correspondences, is a fundamental task in computer vision with many applications. We present a complete solution for this task, including a solution for the minimal problem and the least-squares problem of this task. Previous works mainly focused on finding the global minimizer to address the least-squares problem. However, existing works that show the ability to achieve global minimizer are still unsuitable for real-time applications. Furthermore, as one of contributions of this paper, we prove that there exist ambiguous configurations for any number of lines and planes. These configurations have several solutions in theory, which makes the correct solution may come from a local minimizer. Our algorithm is efficient and able to reveal local minimizers. We employ the Cayley-Gibbs-Rodriguez (CGR) parameterization of the rotation to derive a general rational cost for the three cases of 3D correspondences. The main contribution of this paper is to solve the resulting equation system of the minimal problem and the first-order optimality conditions of the least-squares problem, both of which are of complicated rational forms. The central idea of our algorithm is to introduce intermediate unknowns to simplify the problem. Extensive experimental results show that our algorithm significantly outperforms previous algorithms when the number of correspondences is small. Besides, when the global minimizer is the solution, our algorithm achieves the same accuracy as previous algorithms that have guaranteed global optimality, but our algorithm is applicable to real-time applications.

[1]  Carl Olsson,et al.  Branch-and-Bound Methods for Euclidean Registration Problems , 2009, IEEE Transactions on Pattern Analysis and Machine Intelligence.

[2]  Berthold K. P. Horn,et al.  Closed-form solution of absolute orientation using orthonormal matrices , 1988 .

[3]  Arjan Kuijper,et al.  A universal, closed-form approach for absolute pose problems , 2018, Comput. Vis. Image Underst..

[4]  Yang Gao,et al.  Probabilistic RGB-D Odometry based on Points, Lines and Planes Under Depth Uncertainty , 2017, Robotics Auton. Syst..

[5]  Berthold K. P. Horn,et al.  Closed-form solution of absolute orientation using unit quaternions , 1987 .

[6]  Andrew W. Fitzgibbon,et al.  KinectFusion: Real-time dense surface mapping and tracking , 2011, 2011 10th IEEE International Symposium on Mixed and Augmented Reality.

[7]  Robert Pless,et al.  Extrinsic calibration of a camera and laser range finder (improves camera calibration) , 2004, 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (IEEE Cat. No.04CH37566).

[8]  David A. Cox,et al.  Using Algebraic Geometry , 1998 .

[9]  Andrea Censi,et al.  An ICP variant using a point-to-line metric , 2008, 2008 IEEE International Conference on Robotics and Automation.

[10]  Carl Olsson,et al.  The Registration Problem Revisited: Optimal Solutions From Points, Lines and Planes , 2006, 2006 IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR'06).

[11]  Jiaolong Yang,et al.  Go-ICP: Solving 3D Registration Efficiently and Globally Optimally , 2013, 2013 IEEE International Conference on Computer Vision.

[12]  Javier González,et al.  Extrinsic calibration of a 2d laser-rangefinder and a camera based on scene corners , 2015, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[13]  Vladlen Koltun,et al.  Fast Global Registration , 2016, ECCV.

[14]  K. S. Arun,et al.  Least-Squares Fitting of Two 3-D Point Sets , 1987, IEEE Transactions on Pattern Analysis and Machine Intelligence.

[15]  Yubin Kuang,et al.  Exploiting p-fold symmetries for faster polynomial equation solving , 2012, Proceedings of the 21st International Conference on Pattern Recognition (ICPR2012).

[16]  Javier González,et al.  Convex Global 3D Registration with Lagrangian Duality , 2017, 2017 IEEE Conference on Computer Vision and Pattern Recognition (CVPR).

[17]  Yuichi Taguchi,et al.  P2Pi: A Minimal Solution for Registration of 3D Points to 3D Planes , 2010, ECCV.

[18]  Chen Feng,et al.  Point-plane SLAM for hand-held 3D sensors , 2013, 2013 IEEE International Conference on Robotics and Automation.

[19]  Yuichi Taguchi,et al.  A Theory of Minimal 3D Point to 3D Plane Registration and Its Generalization , 2013, International Journal of Computer Vision.

[20]  Viktor Larsson,et al.  Efficient Solvers for Minimal Problems by Syzygy-Based Reduction , 2017, 2017 IEEE Conference on Computer Vision and Pattern Recognition (CVPR).

[21]  Urbano Nunes,et al.  A Minimal Solution for the Extrinsic Calibration of a Camera and a Laser-Rangefinder , 2012, IEEE Transactions on Pattern Analysis and Machine Intelligence.

[22]  Andreas Geiger,et al.  Vision meets robotics: The KITTI dataset , 2013, Int. J. Robotics Res..

[23]  Pavel Zemcík,et al.  Absolute pose estimation from line correspondences using direct linear transformation , 2016, Comput. Vis. Image Underst..

[24]  Reinhard Koch,et al.  An efficient and robust line segment matching approach based on LBD descriptor and pairwise geometric consistency , 2013, J. Vis. Commun. Image Represent..

[25]  Ji Zhang,et al.  LOAM: Lidar Odometry and Mapping in Real-time , 2014, Robotics: Science and Systems.

[26]  Giorgio Grisetti,et al.  NICP: Dense normal based point cloud registration , 2015, 2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[27]  Homer H. Chen Pose Determination from Line-to-Plane Correspondences: Existence Condition and Closed-Form Solutions , 1991, IEEE Trans. Pattern Anal. Mach. Intell..

[28]  Andrew W. Fitzgibbon,et al.  Efficient Intersection of Three Quadrics and Applications in Computer Vision , 2016, 2016 IEEE Conference on Computer Vision and Pattern Recognition (CVPR).

[29]  Rafael Grompone von Gioi,et al.  LSD: A Fast Line Segment Detector with a False Detection Control , 2010, IEEE Transactions on Pattern Analysis and Machine Intelligence.

[30]  Arjan Kuijper,et al.  Unifying Algebraic Solvers for Scaled Euclidean Registration from Point, Line and Plane Constraints , 2016, ACCV.

[31]  Lipu Zhou,et al.  Automatic Extrinsic Calibration of a Camera and a 3D LiDAR Using Line and Plane Correspondences , 2018, 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[32]  Robert C. Bolles,et al.  Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography , 1981, CACM.

[33]  Viktor Larsson,et al.  Uncovering Symmetries in Polynomial Systems , 2016, ECCV.

[34]  Lipu Zhou,et al.  A New Minimal Solution for the Extrinsic Calibration of a 2D LIDAR and a Camera Using Three Plane-Line Correspondences , 2014, IEEE Sensors Journal.

[35]  Laurent Kneip,et al.  OpenGV: A unified and generalized approach to real-time calibrated geometric vision , 2014, 2014 IEEE International Conference on Robotics and Automation (ICRA).

[36]  Ji Zhang,et al.  Visual-lidar odometry and mapping: low-drift, robust, and fast , 2015, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[37]  Darius Burschka,et al.  Stochastic global optimization for robust point set registration , 2011, Comput. Vis. Image Underst..

[38]  Joel A. Hesch,et al.  A Direct Least-Squares (DLS) method for PnP , 2011, 2011 International Conference on Computer Vision.

[39]  Hongdong Li,et al.  UPnP: An Optimal O(n) Solution to the Absolute Pose Problem with Universal Applicability , 2014, ECCV.

[40]  Paul J. Besl,et al.  Method for registration of 3-D shapes , 1992, Other Conferences.

[41]  Reinhard Koch,et al.  Pose Estimation from Line Correspondences: A Complete Analysis and a Series of Solutions , 2017, IEEE Transactions on Pattern Analysis and Machine Intelligence.

[42]  Kostas Daniilidis,et al.  Automatic alignment of a camera with a line scan LIDAR system , 2011, 2011 IEEE International Conference on Robotics and Automation.

[43]  Hongdong Li,et al.  The 3D-3D Registration Problem Revisited , 2007, 2007 IEEE 11th International Conference on Computer Vision.

[44]  Anders P. Eriksson,et al.  Solving quadratically constrained geometrical problems using lagrangian duality , 2008, 2008 19th International Conference on Pattern Recognition.

[45]  Stergios I. Roumeliotis,et al.  Optimal estimation of vanishing points in a Manhattan world , 2011, 2011 International Conference on Computer Vision.

[46]  Andreas Birk,et al.  Fast plane detection and polygonalization in noisy 3D range images , 2008, 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[47]  Gary R. Bradski,et al.  ORB: An efficient alternative to SIFT or SURF , 2011, 2011 International Conference on Computer Vision.