iSAM: Fast Incremental Smoothing and Mapping with Efficient Data Association

We introduce incremental smoothing and mapping (iSAM), a novel approach to the problem of simultaneous localization and mapping (SLAM) that addresses the data association problem and allows real-time application in large-scale environments. We employ smoothing to obtain the complete trajectory and map without the need for any approximations, exploiting the natural sparsity of the smoothing information matrix. A QR-factorization of this information matrix is at the heart of our approach. It provides efficient access to the exact covariances as well as to conservative estimates that are used for online data association. It also allows recovery of the exact trajectory and map at any given time by back-substitution. Instead of refactoring in each step, we update the QR-factorization whenever a new measurement arrives. We analyze the effect of loops, and show how our approach extends to the non-linear case. Finally, we provide experimental validation of the overall non-linear algorithm based on the standard Victoria Park data set with unknown correspondences.

[1]  Frank Dellaert,et al.  Square Root SAM , 2005, Robotics: Science and Systems.

[2]  A. Volgenant,et al.  A shortest augmenting path algorithm for dense and sparse linear assignment problems , 1987, Computing.

[3]  Mark A. Paskin,et al.  Thin Junction Tree Filters for Simultaneous Localization and Mapping , 2002, IJCAI.

[4]  Peter Cheeseman,et al.  On the Representation and Estimation of Spatial Uncertainty , 1986 .

[5]  Sebastian Thrun,et al.  Robotic mapping: a survey , 2003 .

[6]  Udo Frese,et al.  Closing a Million-Landmarks Loop , 2006, 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[7]  Gerhard Lakemeyer,et al.  Exploring artificial intelligence in the new millennium , 2003 .

[8]  Frank Dellaert,et al.  Fast Incremental Square Root Information Smoothing , 2007, IJCAI.

[9]  Kurt Konolige,et al.  Incremental mapping of large cyclic environments , 1999, Proceedings 1999 IEEE International Symposium on Computational Intelligence in Robotics and Automation. CIRA'99 (Cat. No.99EX375).

[10]  Tom Duckett,et al.  A multilevel relaxation algorithm for simultaneous localization and mapping , 2005, IEEE Transactions on Robotics.

[11]  Hanumant Singh,et al.  Visually Navigating the RMS Titanic with SLAM Information Filters , 2005, Robotics: Science and Systems.

[12]  Andrew W. Fitzgibbon,et al.  Bundle Adjustment - A Modern Synthesis , 1999, Workshop on Vision Algorithms.

[13]  N. Nathan.  Self and will , 1997 .

[14]  Michael Bosse,et al.  An Atlas framework for scalable mapping , 2003, 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422).

[15]  Udo Frese.  Treemap: An O(log n) algorithm for indoor simultaneous localization and mapping , 2006, Auton. Robots.

[16]  Peter Cheeseman,et al.  A stochastic map for uncertain spatial relationships , 1988 .

[17]  Bernhard P. Wrobel,et al.  Multiple View Geometry in Computer Vision , 2001 .

[18]  S. Thrun,et al.  Monte carlo em for data-association and its applications in computer vision , 2001 .

[19]  Bernhard Nebel,et al.  Navigation mobiler Roboter mit Laserscans , 1997, AMS.

[20]  Kurt Konolige,et al.  Large-Scale Map-Making , 2004, AAAI.

[21]  Frank Dellaert,et al.  Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing , 2006, Int. J. Robotics Res..

[22]  Henrik I. Christensen,et al.  Graphical SLAM - a self-correcting map , 2004, IEEE International Conference on Robotics and Automation, 2004. Proceedings. ICRA '04. 2004.

[23]  Gene H. Golub,et al.  Matrix computations (3rd ed.) , 1996 .

[24]  G. Golub,et al.  Large scale geodetic least squares adjustment by dissection and orthogonal decomposition , 1979 .

[25]  Jeffrey K. Uhlmann,et al.  A counter example to the theory of simultaneous localization and map building , 2001, Proceedings 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No.01CH37164).

[26]  Ingemar J. Cox,et al.  Dynamic Map Building for an Autonomous Mobile Robot , 1990, EEE International Workshop on Intelligent Robots and Systems, Towards a New Frontier of Applications.

[27]  Timothy A. Davis,et al.  A column approximate minimum degree ordering algorithm , 2000, TOMS.

[28]  Frank Dellaert,et al.  Spectral partitioning for structure from motion , 2003, Proceedings Ninth IEEE International Conference on Computer Vision.

[29]  Evangelos E. Milios,et al.  Globally Consistent Range Scan Alignment for Environment Mapping , 1997, Auton. Robots.