A closed-form estimate of 3D ICP covariance

We present a closed-form solution to estimate the covariance of the resultant transformation provided by the Iterative Closest Point (ICP) algorithm for 3D point cloud registration. We extend an existing work [1] that estimates ICP's covariance in 2D with point to plane error metric to 3D with point to point and point to plane error metrics. Moreover, we do not make any assumption on the noise present in the sensor data and have no constraints on the estimated rigid transformation. The source code of our implementation is made publicly available, which can be adapted to work for ICP with different error metrics with minor changes. Our preliminary results show that ICP's covariance is lower at a global minimum than at a local minima.

[1]  Kok-Lim Low.  Linear Least-Squares Optimization for Point-to-Plane ICP Surface Registration , 2004 .

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

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

[4]  Weisi Lin,et al.  Sparse Depth Odometry: 3D keypoint based pose estimation from dense depth data , 2015, 2015 IEEE International Conference on Robotics and Automation (ICRA).

[5]  Federico Tombari,et al.  SHOT: Unique signatures of histograms for surface and texture description , 2014, Comput. Vis. Image Underst..

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