The Bayes Tree: An Algorithmic Foundation for Probabilistic Robot Mapping

We present a novel data structure, the Bayes tree, that provides an algorithmic foundation enabling a better understanding of existing graphical model inference algorithms and their connection to sparse matrix factorization methods. Similar to a clique tree, a Bayes tree encodes a factored probability density, but unlike the clique tree it is directed and maps more naturally to the square root information matrix of the simultaneous localization and mapping (SLAM) problem. In this paper, we highlight three insights provided by our new data structure. First, the Bayes tree provides a better understanding of batch matrix factorization in terms of probability densities. Second, we show how the fairly abstract updates to a matrix factorization translate to a simple editing of the Bayes tree and its conditional densities. Third, we apply the Bayes tree to obtain a completely novel algorithm for sparse nonlinear incremental optimization, that combines incremental updates with fluid relinearization of a reduced set of variables for efficiency, combined with fast convergence to the exact solution. We also present a novel strategy for incremental variable reordering to retain sparsity.We evaluate our algorithmon standard datasets in both landmark and pose SLAM settings.

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

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

[3]  Edwin Olson,et al.  Fast iterative alignment of pose graphs with poor initial estimates , 2006, Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006..

[4]  Robert E. Tarjan,et al.  Simple Linear-Time Algorithms to Test Chordality of Graphs, Test Acyclicity of Hypergraphs, and Selectively Reduce Acyclic Hypergraphs , 1984, SIAM J. Comput..

[5]  Frank Dellaert,et al.  Covariance recovery from a square root information matrix for data association , 2009, Robotics Auton. Syst..

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

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

[8]  B. Hall Lie Groups, Lie Algebras, and Representations: An Elementary Introduction , 2004 .

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

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

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

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

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

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

[15]  Frank Dellaert,et al.  iSAM: Incremental Smoothing and Mapping , 2008, IEEE Transactions on Robotics.

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

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

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

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

[20]  Gene H. Golub,et al.  Methods for modifying matrix factorizations , 1972, Milestones in Matrix Computation.

[21]  Frank Dellaert,et al.  Loopy SAM , 2007, IJCAI.

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

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

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

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

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

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

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

[29]  Lina María Paz,et al.  Large-Scale 6-DOF SLAM With Stereo-in-Hand , 2008, IEEE Transactions on Robotics.

[30]  Judea Pearl,et al.  Probabilistic reasoning in intelligent systems - networks of plausible inference , 1991, Morgan Kaufmann series in representation and reasoning.

[31]  Carl Friedrich Gauss Theoria motus corporum coelestium , 1981 .

[32]  W. E. Gentleman Least Squares Computations by Givens Transformations Without Square Roots , 1973 .

[33]  Wolfram Burgard,et al.  Probabilistic Robotics (Intelligent Robotics and Autonomous Agents) , 2005 .

[34]  Henrik I. Christensen,et al.  Closing the Loop With Graphical SLAM , 2007, IEEE Transactions on Robotics.

[35]  Michael I. Jordan,et al.  Probabilistic Networks and Expert Systems , 1999 .

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