Square Root SAM: Simultaneous Localization and Mapping via Square Root Information Smoothing

Solving the SLAM (simultaneous localization and mapping) problem is one way to enable a robot to explore, map, and navigate in a previously unknown environment. Smoothing approaches have been investigated as a viable alternative to extended Kalman filter (EKF)-based solutions to the problem. In particular, approaches have been looked at that factorize either the associated information matrix or the measurement Jacobian into square root form. Such techniques have several significant advantages over the EKF: they are faster yet exact; they can be used in either batch or incremental mode; are better equipped to deal with non-linear process and measurement models; and yield the entire robot trajectory, at lower cost for a large class of SLAM problems. In addition, in an indirect but dramatic way, column ordering heuristics automatically exploit the locality inherent in the geographic nature of the SLAM problem. This paper presents the theory underlying these methods, along with an interpretation of factorization in terms of the graphical model associated with the SLAM problem. Both simulation results and actual SLAM experiments in large-scale environments are presented that underscore the potential of these methods as an alternative to EKF-based approaches.

[1]  Hanumant Singh,et al.  Exactly Sparse Delayed-State Filters , 2005, Proceedings of the 2005 IEEE International Conference on Robotics and Automation.

[2]  F. Dellaert,et al.  Exploiting Locality by Nested Dissection For Square Root Smoothing and Mapping , 2005 .

[3]  Raja Chatila,et al.  Stochastic multisensory data fusion for mobile robot location and environment modeling , 1989 .

[4]  S. I. Granshaw,et al.  BUNDLE ADJUSTMENT METHODS IN ENGINEERING PHOTOGRAMMETRY , 2006 .

[5]  Henrik I. Christensen,et al.  Graphical SLAM using vision and the measurement subspace , 2005, 2005 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[6]  Timothy A. Davis,et al.  Algorithm 8 xx : a concise sparse Cholesky factorization package , 2004 .

[7]  Tom Duckett,et al.  A Multigrid Approach for Acceleration Relaxation-Based SLAM , 2003, AMS.

[8]  Soren W. Henriksen,et al.  Manual of photogrammetry , 1980 .

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

[10]  Jean-Paul Laumond,et al.  Position referencing and consistent world modeling for mobile robots , 1985, Proceedings. 1985 IEEE International Conference on Robotics and Automation.

[11]  John J. Leonard,et al.  Decoupled stochastic mapping [for mobile robot & AUV navigation] , 2001 .

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

[13]  P. Heggernes,et al.  Finding Good Column Orderings for Sparse QR Factorization , 1996 .

[14]  Paul Newman,et al.  On the Structure and Solution of the Simultaneous Localisation and Map Building Problem , 1999 .

[15]  B. Peyton,et al.  An Introduction to Chordal Graphs and Clique Trees , 1993 .

[16]  Stefan B. Williams,et al.  An efficient approach to the simultaneous localisation and mapping problem , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[17]  David J. Spiegelhalter,et al.  Probabilistic Networks and Expert Systems , 1999, Information Science and Statistics.

[18]  Gene H. Golub,et al.  Matrix computations , 1983 .

[19]  Hugh F. Durrant-Whyte,et al.  Simultaneous Localization and Mapping with Sparse Extended Information Filters , 2004, Int. J. Robotics Res..

[20]  Sebastian Thrun,et al.  FastSLAM: a factored solution to the simultaneous localization and mapping problem , 2002, AAAI/IAAI.

[21]  Martial Hebert,et al.  Experimental Comparison of Techniques for Localization and Mapping Using a Bearing-Only Sensor , 2000, ISER.

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

[23]  Agustín Jiménez,et al.  Local maps fusion for real time multirobot indoor simultaneous localization and mapping , 2004, IEEE International Conference on Robotics and Automation, 2004. Proceedings. ICRA '04. 2004.

[24]  John J. Leonard,et al.  Consistent, Convergent, and Constant-Time SLAM , 2003, IJCAI.

[25]  Stephen R. Marsland,et al.  Learning globally consistent maps by relaxation , 2000, Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065).

[26]  Alex Pothen,et al.  Distributed Multifrontal Factorization Using Clique Trees , 1991, SIAM Conference on Parallel Processing for Scientific Computing.

[27]  G. Bierman Factorization methods for discrete sequential estimation , 1977 .

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

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

[30]  Rodney A. Brooks,et al.  Visual map making for a mobile robot , 1985, Proceedings. 1985 IEEE International Conference on Robotics and Automation.

[31]  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).

[32]  WILLIAM W. HAGER,et al.  Modifying a Sparse Cholesky Factorization | SIAM Journal on Matrix Analysis and Applications | Vol. 20, No. 3 | Society for Industrial and Applied Mathematics , 1999 .

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

[34]  Patrick R. Amestoy,et al.  An Approximate Minimum Degree Ordering Algorithm , 1996, SIAM J. Matrix Anal. Appl..

[35]  Wilfried Brauer,et al.  Spatial Cognition III , 2003, Lecture Notes in Computer Science.

[36]  Matteo Golfarelli,et al.  Elastic correction of dead-reckoning errors in map building , 1998, Proceedings. 1998 IEEE/RSJ International Conference on Intelligent Robots and Systems. Innovations in Theory, Practice and Applications (Cat. No.98CH36190).

[37]  Eduardo Mario Nebot,et al.  Optimization of the simultaneous localization and map-building algorithm for real-time implementation , 2001, IEEE Trans. Robotics Autom..

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

[39]  Udo Frese Treemap: An O(log n) Algorithm for Simultaneous Localization and Mapping , 2004, Spatial Cognition.

[40]  W. Freeman,et al.  Generalized Belief Propagation , 2000, NIPS.

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

[42]  Kevin P. Murphy,et al.  Bayesian Map Learning in Dynamic Environments , 1999, NIPS.

[43]  Jeffrey K. Uhlmann,et al.  Simultaneous localisation and map building using split covariance intersection , 2001, Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the the Next Millennium (Cat. No.01CH37180).

[44]  Gerhard Winkler,et al.  Image analysis, random fields and dynamic Monte Carlo methods: a mathematical introduction , 1995, Applications of mathematics.

[45]  Matteo Golfarelli,et al.  Correction of dead-reckoning errors in map building for mobile robots , 2001, IEEE Trans. Robotics Autom..

[46]  R. Tarjan,et al.  A Separator Theorem for Planar Graphs , 1977 .

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

[48]  Peter C. Cheeseman,et al.  Estimating uncertain spatial relationships in robotics , 1986, Proceedings. 1987 IEEE International Conference on Robotics and Automation.

[49]  Gaurav S. Sukhatme,et al.  Relaxation on a mesh: a formalism for generalized localization , 2001, Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Expanding the Societal Role of Robotics in the the Next Millennium (Cat. No.01CH37180).

[50]  John J. Leonard,et al.  Directed Sonar Sensing for Mobile Robot Navigation , 1992 .

[51]  Olivier D. Faugeras,et al.  Maintaining representations of the environment of a mobile robot , 1988, IEEE Trans. Robotics Autom..

[52]  Frank Dellaert,et al.  Exploiting Locality in SLAM by Nested Dissection , 2006, Robotics: Science and Systems.

[53]  Udo Frese An O(log n) algorithm for simultaneous localization and mapping of mobile robots in indoor environments , 2004 .

[54]  Favio R. Masson,et al.  Navigation and Mapping in Large Unstructured Environments , 2004, Int. J. Robotics Res..

[55]  Ingemar J. Cox,et al.  Autonomous Robot Vehicles , 1990, Springer New York.

[56]  John J. Leonard,et al.  Robust Mapping and Localization in Indoor Environments Using Sonar Data , 2002, Int. J. Robotics Res..

[57]  Evangelos E. Milios,et al.  Robot Pose Estimation in Unknown Environments by Matching 2D Range Scans , 1994, 1994 Proceedings of IEEE Conference on Computer Vision and Pattern Recognition.

[58]  J. Leonard,et al.  Decoupled Stochastic Mapping , 2001 .

[59]  Richard Szeliski,et al.  Recovering 3D Shape and Motion from Image Streams Using Nonlinear Least Squares , 1994, J. Vis. Commun. Image Represent..

[60]  Stephen R. Marsland,et al.  Fast, On-Line Learning of Globally Consistent Maps , 2002, Auton. Robots.

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

[62]  Ingemar J. Cox,et al.  Dynamic Map Building for an Autonomous Mobile Robot , 1992 .

[63]  Pontus Matstoms,et al.  Sparse QR factorization in MATLAB , 1994, TOMS.

[64]  C. S. George Lee,et al.  A geometric feature relation graph formulation for consistent sensor fusion , 1990, 1990 IEEE International Conference on Systems, Man, and Cybernetics Conference Proceedings.

[65]  Richard Szeliski,et al.  Vision Algorithms: Theory and Practice , 2002, Lecture Notes in Computer Science.

[66]  J. M. M. Montiel,et al.  The SPmap: a probabilistic framework for simultaneous localization and map building , 1999, IEEE Trans. Robotics Autom..

[67]  John E. Dennis,et al.  Numerical methods for unconstrained optimization and nonlinear equations , 1983, Prentice Hall series in computational mathematics.

[68]  O. Faugeras Three-dimensional computer vision: a geometric viewpoint , 1993 .

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

[70]  D. Rose,et al.  Generalized nested dissection , 1977 .

[71]  Randall Smith,et al.  Estimating uncertain spatial relationships in robotics , 1986, Proceedings. 1987 IEEE International Conference on Robotics and Automation.

[72]  K. Atkinson Close Range Photogrammetry and Machine Vision , 1996 .

[73]  Timothy A. Davis,et al.  Modifying a Sparse Cholesky Factorization , 1999, SIAM J. Matrix Anal. Appl..

[74]  Martial Hebert,et al.  Invariant filtering for simultaneous localization and mapping , 2000, Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065).

[75]  X. Jin Factor graphs and the Sum-Product Algorithm , 2002 .

[76]  Brendan J. Frey,et al.  Factor graphs and the sum-product algorithm , 2001, IEEE Trans. Inf. Theory.

[77]  Hugh F. Durrant-Whyte,et al.  Simultaneous map building and localization for an autonomous mobile robot , 1991, Proceedings IROS '91:IEEE/RSJ International Workshop on Intelligent Robots and Systems '91.

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

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

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

[81]  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).

[82]  Raja Chatila,et al.  An Experimental System for Incremental Environment Modelling by an Autonomous Mobile Robot , 1989, ISER.

[83]  Frank Dellaert,et al.  A Multifrontal QR Factorization Approach to Distributed Inference Applied to Multirobot Localization and Mapping , 2005, AAAI.

[84]  S. B. Kang,et al.  Recovering 3 D Shape and Motion from Image Streams using Non-Linear Least Squares , 1993 .

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

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

[87]  Sebastian Thrun,et al.  Probabilistic robotics , 2002, CACM.