Accurate Covariance Estimation for Pose Data from Iterative Closest Point Algorithm

One of the fundamental problems of robotics and navigation is the estimation of relative pose of an external object with respect to the observer. A common method for computing the relative pose is the Iterative Closest Point (ICP) algorithm, where a reference point cloud of a known object is registered against a sensed point cloud to determine relative pose. To use this computed pose information in down-stream processing algorithms, it is necessary to estimate the uncertainty of the ICP output, typically represented as a covariance matrix. In this paper we introduce a novel method for estimating uncertainty from sensed data. Using a visual simulation of an automated aerial refueling (AAR) task, we demonstrate significantly more accurate uncertainty estimates using our proposed approach than a naive “Jacobian” method.

[1]  F. Markley,et al.  Quaternion Attitude Estimation Using Vector Observations , 2000 .

[2]  D. Scharstein,et al.  A Taxonomy and Evaluation of Dense Two-Frame Stereo Correspondence Algorithms , 2001, Proceedings IEEE Workshop on Stereo and Multi-Baseline Vision (SMBV 2001).

[3]  Roland Siegwart,et al.  A Review of Point Cloud Registration Algorithms for Mobile Robotics , 2015, Found. Trends Robotics.

[4]  Weisi Lin,et al.  A closed-form estimate of 3D ICP covariance , 2015, 2015 14th IAPR International Conference on Machine Vision Applications (MVA).

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

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

[7]  S. Bonnabel,et al.  A New Approach to 3D ICP Covariance Estimation , 2019, IEEE Robotics and Automation Letters.

[8]  Paul J. Besl,et al.  A Method for Registration of 3-D Shapes , 1992, IEEE Trans. Pattern Anal. Mach. Intell..

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

[10]  Marc Levoy,et al.  Efficient variants of the ICP algorithm , 2001, Proceedings Third International Conference on 3-D Digital Imaging and Modeling.

[11]  Anton Wellstein,et al.  7. PERFORMING ORGANIZATION NAME(S) AND ADDRESS(ES) 8. PERFORMING ORGANIZATION REPORT NUMBER , 2005 .

[12]  Andrea Censi,et al.  An accurate closed-form estimate of ICP's covariance , 2007, Proceedings 2007 IEEE International Conference on Robotics and Automation.

[13]  Arthur G. O. Mutambara,et al.  Decentralized Estimation and Control for Multisensor Systems , 2019 .

[14]  David M. Chelberg,et al.  JPALS Visualization Tool , 2012, 2012 IEEE/ACM 16th International Symposium on Distributed Simulation and Real Time Applications.

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

[16]  Nancy Argüelles,et al.  Author ' s , 2008 .

[17]  François Pomerleau,et al.  CELLO-3D: Estimating the Covariance of ICP in the Real World , 2018, 2019 International Conference on Robotics and Automation (ICRA).

[18]  Peter S. Maybeck,et al.  Stochastic Models, Estimation And Control , 2012 .

[19]  Scott Nykl,et al.  Combining Stereo Vision and Inertial Navigation for Automated Aerial Refueling , 2017 .