Accurate 3D maps from depth images and motion sensors via nonlinear Kalman filtering

This paper investigates the use of depth images as localisation sensors for 3D map building. The localisation information is derived from the 3D data thanks to the ICP (Iterative Closest Point) algorithm. The covariance of the ICP, and thus of the localization error, is analysed, and described by a Fisher Information Matrix. It is advocated this error can be much reduced if the data is fused with measurements from other motion sensors, or even with prior knowledge on the motion. The data fusion is performed by a recently introduced specific extended Kalman filter, the so-called Invariant EKF, and is directly based on the estimated covariance of the ICP. The resulting filter is natural, and is proved to possess strong properties. Experiments with a Kinect sensor and a three-axis gyroscope prove clear improvement in the accuracy of the localization, and thus in the accuracy of the built 3D map.

[1]  Philippe Martin,et al.  Invariant Extended Kalman Filter: theory and application to a velocity-aided attitude estimation problem , 2009, Proceedings of the 48h IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference.

[2]  P. Bougerol.  Kalman filtering with random coefficients and contractions , 1993 .

[3]  G. Goodwin,et al.  Connection between continuous and discrete Riccati equations with applications to kalman filtering , 1988 .

[4]  Philippe Martin,et al.  Non-Linear Symmetry-Preserving Observers on Lie Groups , 2007, IEEE Transactions on Automatic Control.

[5]  François Goulette,et al.  Co-registration of DSM and 3D points clouds acquired by a Mobile Mapping System , 2009 .

[6]  Silvere Bonnabel,et al.  Left-invariant extended Kalman filter and attitude estimation , 2007, 2007 46th IEEE Conference on Decision and Control.

[7]  Andreas Birk,et al.  Fast Registration Based on Noisy Planes With Unknown Correspondences for 3-D Mapping , 2010, IEEE Transactions on Robotics.

[8]  Hugh F. Durrant-Whyte,et al.  A solution to the simultaneous localization and map building (SLAM) problem , 2001, IEEE Trans. Robotics Autom..

[9]  Agostino Martinelli,et al.  Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale, and Bias Determination , 2012, IEEE Transactions on Robotics.

[10]  Robert E. Mahony,et al.  Nonlinear Complementary Filters on the Special Orthogonal Group , 2008, IEEE Transactions on Automatic Control.

[11]  R. E. Kalman,et al.  New Results in Linear Filtering and Prediction Theory , 1961 .

[12]  F. Goulette,et al.  An integrated on-board laser range sensing system for on-the-way city and road modelling , 2006 .

[13]  Rita Cunha,et al.  A nonlinear position and attitude observer on SE(3) using landmark measurements , 2010, Syst. Control. Lett..

[14]  John L. Crassidis,et al.  Survey of nonlinear attitude estimation methods , 2007 .

[15]  Zhongjie,et al.  Implementation of a 3 D ICP-based Scan Matcher , 2010 .

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

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

[18]  Robert E. Mahony,et al.  Robust Nonlinear Fusion of Inertial and Visual Data for position, velocity and attitude estimation of UAV , 2007, Proceedings 2007 IEEE International Conference on Robotics and Automation.