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 the 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, named iSAM2, which achieves improvements in efficiency through incremental variable re-ordering and fluid relinearization, eliminating the need for periodic batch steps. We analyze various properties of iSAM2 in detail, and show on a range of real and simulated datasets that our algorithm compares favorably with other recent mapping algorithms in both quality and efficiency.
ArnborgSCorneilDProskurowskiA (1987) Complexity of finding embeddings in a k-tree. SIAM J Algebraic Discrete Meth8: 277–284.
2.
BlairJPeytonB (1993) An introduction to chordal graphs and clique trees. In GeorgeJGilbertJLiuJ-H (eds), Graph Theory and Sparse Matrix Computations (IMA Volumes in Mathematics and its Applications, vol. 56). New York: Springer-Verlag, pp. 1–27.
3.
BosseMNewmanPLeonardJTellerS (2004) Simultaneous localization and map building in large-scale cyclic environments using the Atlas framework. Int J Robotics Res23: 1113–1139.
4.
BrooksR (1985) Visual map making for a mobile robot. In IEEE International Conference on Robotics and Automation (ICRA), vol. 2, pp. 824–829.
5.
CastellanosJMontielJNeiraJTardósJ (1999) The SPmap: A probabilistic framework for simultaneous localization and map building. IEEE Trans Robot Automat15: 948–953.
6.
ChenYDavisTHagerWRajamanickamS (2008) Algorithm 887: CHOLMOD, supernodal sparse Cholesky factorization and update/downdate. ACM Trans Math Softw35(3): 22:1–22:14.
7.
CowellRDawidALauritzenSSpiegelhalterD (1999) Statistics for engineering and information science. Probabilistic Networks and Expert Systems. Berlin: Springer-Verlag.
8.
DavisTGilbertJLarimoreSNgE (2004) A column approximate minimum degree ordering algorithm. ACM Trans Math Softw30: 353–376.
9.
DellaertF (2005) Square Root SAM: Simultaneous location and mapping via square root information smoothing. In Robotics: Science and Systems (RSS).
10.
DellaertFCarlsonJIlaVNiKThorpeC (2010) Subgraph-preconditioned conjugate gradient for large scale SLAM. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).
11.
DellaertFKaessM (2006) Square Root SAM: Simultaneous localization and mapping via square root information smoothing. Int J Robotics Res25: 1181–1203.
12.
DellaertFKippAKrauthausenP (2005) A multifrontal QR factorization approach to distributed inference applied to multi-robot localization and mapping. In Proceedings 22nd AAAI National Conference on AI, Pittsburgh, PA.
13.
DissanayakeMNewmanPDurrant-WhyteHClarkSCsorbaM (2001) A solution to the simultaneous localization and map building (SLAM) problem. IEEE Trans Robot Automat17: 229–241.
EadeEDrummondT (2007) Monocular SLAM as a graph of coalesced observations. In International Conference on Computer Vision (ICCV), Rio de Janeiro, Brazil, Oct 2007, pp. 1–8
16.
FolkessonJChristensenH (2004) Graphical SLAM–-a self-correcting map. In IEEE International Conference on Robotics and Automation (ICRA), vol. 1, pp. 383–390.
17.
FolkessonJChristensenH (2007) Closing the loop with Graphical SLAM. IEEE Trans Robotics23: 731–741.
18.
FolkessonJLeonardJLeederkerkenJWilliamsR (2007) Feature tracking for underwater navigation using sonar. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 3678–3684.
19.
FreseU (2006) Treemap: An O(logn) algorithm for indoor simultaneous localization and mapping. Autonomous Robots21: 103–122.
20.
FreseULarssonPDuckettT (2005) A multilevel relaxation algorithm for simultaneous localisation and mapping. IEEE Trans Robotics21: 196–207.
21.
GaussC (1809) Theoria Motus Corporum Coelestium in Sectionibus Conicis Solem Mabientium [Theory of the Motion of the Heavenly Bodies Moving about the Sun in Conic Sections]. Hamburg: Perthes and Besser. English translation available at http://name.umdl.umich.edu/AGG8895.0001.001.
22.
GentlemanW (1973) Least squares computations by Givens transformations without square roots. IMA J Appl Math12: 329–336.
23.
GillPGolubGMurrayWSaundersM (1974) Methods for modifying matrix factorizations. Maths Computat28: 505–535.
GrassiaFS (1998) Practical parameterization of rotations using the exponential map. J Graph Tools3: 29–48.
26.
GrisettiGKuemmerleRStachnissCFreseUHertzbergC (2010) Hierarchical optimization on manifolds for online 2D and 3D mapping. In IEEE Intl. Conf. on Robotics and Automation (ICRA), Anchorage, AK.
27.
GrisettiGStachnissCGrzonkaSBurgardW (2007) A tree parameterization for efficiently computing maximum likelihood maps using gradient descent. In Robotics: Science and Systems (RSS).
28.
GutmannJ-SNebelB (1997) Navigation mobiler Roboter mit Laserscans. In Autonome Mobile Systeme. Berlin: Springer-Verlag.
29.
HallB (2000) Lie Groups, Lie Algebras, and Representations: An Elementary Introduction. Berlin: Springer.
30.
HartleyRZissermanA (2000) Multiple View Geometry in Computer Vision. Cambridge: Cambridge University Press.
31.
HeggernesPMatstomsP (1996) Finding good column orderings for sparse QR factorization. In Second SIAM Conference on Sparse Matrices.
32.
HowardASukhatmeGMatarićM (2006) Multi-robot mapping using manifold representations. Proc IEEE9: 1360–1369.
33.
JulierSUhlmannJ (2001) A counter example to the theory of simultaneous localization and map building. In IEEE International Conference on Robotics and Automation (ICRA), vol. 4, pp. 4238–4243.
34.
KaessMDellaertF (2009) Covariance recovery from a square root information matrix for data association. J Robotics Auton Syst57: 1198–1210.
35.
KaessMIlaVRobertsRDellaertF (2010) The Bayes tree: An algorithmic foundation for probabilistic robot mapping. In International Workshop on the Algorithmic Foundations of Robotics, Singapore, pp. 157–173.
36.
KaessMJohannssonHRobertsRIlaVLeonardJDellaertF (2011) iSAM2: Incremental smoothing and mapping with fluid relinearization and incremental variable reordering. In IEEE International Conference on Robotics and Automation (ICRA), Shanghai, China.
37.
KaessMRanganathanADellaertF (2007) Fast incremental square root information smoothing. In International Joint Conference on AI (IJCAI), Hyderabad, India, pp. 2129–2134.
38.
KaessMRanganathanADellaertF (2008) iSAM: Incremental smoothing and mapping. IEEE Trans Robotics24: 1365–1378.
39.
KimBKaessMFletcherLLeonardJBachrachARoyN. (2010) Multiple relative pose graphs for robust cooperative mapping. In IEEE International Conference on Robotics and Automation (ICRA), Anchorage, Alaska, pp. 3185–3192.
40.
KollerDFriedmanN (2009) Probabilistic Graphical Models: Principles and Techniques. Cambridge, MA: The MIT Press.
41.
KonoligeK (2004) Large-scale map-making. In Proceedings 21st AAAI National Conference on AI, San Jose, CA.
42.
KonoligeKAgrawalM (2008) FrameSLAM: from bundle adjustment to realtime visual mapping. IEEE Trans Robotics24: 1066–1077.
43.
KonoligeKGrisettiGKuemmerleRBurgardWBensonLVincentR (2010) Sparse pose adjustment for 2D mapping. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).
44.
KrauthausenPDellaertFKippA (2006) Exploiting locality by nested dissection for square root smoothing and mapping. In Robotics: Science and Systems (RSS).
45.
KschischangFFreyBLoeligerH-A (2001) Factor graphs and the sum-product algorithm. IEEE Trans Inform Theory47(2): 498–519.
LuFMiliosE (1997) Globally consistent range scan alignment for environment mapping. In Autonomous Robots, pp. 333–349.
48.
MahonIWilliamsSPizarroOJohnson-RobersonM (2008) Efficient view-based SLAM using visual loop closures. IEEE Trans Robotics24: 1002–1014.
49.
NiKDellaertF (2010) Multi-level submap based SLAM using nested dissection. In IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).
50.
NiKSteedlyDDellaertF (2007) Tectonic SAM: Exact; out-of-core; submap-based SLAM. In IEEE International Conference on Robotics and Automation (ICRA), Rome, Italy.
51.
OlsonELeonardJTellerS (2006) Fast iterative alignment of pose graphs with poor initial estimates. In IEEE International Conference on Robotics and Automation (ICRA).
52.
PaskinM (2003) Thin junction tree filters for simultaneous localization and mapping. In International Joint Conference on AI (IJCAI).
53.
PazLPiniesPTardósJNeiraJ (2008) Large scale 6DOF SLAM with stereo-in-hand. IEEE Trans Robotics24: 946–957.
54.
PearlJ (1988) Probabilistic Reasoning in Intelligent Systems: Networks of Plausible Inference. San Francisco, CA: Morgan Kaufmann.
55.
PothenASunC (1992) Distributed multifrontal factorization using clique trees. In Proceedings of the Fifth SIAM Conference on Parallel Processing for Scientific Computing. Philadelphia, PA: Society for Industrial and Applied Mathematics, pp. 34–40.
56.
RanganathanAKaessMDellaertF (2007) Loopy SAM. In International Joint Conference on AI (IJCAI), Hyderabad, India, pp. 2191–2196.
57.
SibleyGMeiCReidINewmanP (2009) Adaptive relative bundle adjustment. In Robotics: Science and Systems (RSS).
58.
SmithRSelfMCheesemanP (1987) A stochastic map for uncertain spatial relationships. In International Symposium on Robotics Research.
59.
StrasdatHMontielJDavisonA (2010) Real-time monocular SLAM: Why filter? In IEEE International Conference on Robotics and Automation (ICRA).
60.
TarjanRYannakakisM (1984) Simple linear-time algorithms to test chordality of graphs, test acyclicity of hypergraphs and selectively reduce acyclic hypergraphs. SIAM J Comput13: 566–579.
61.
ThrunSBurgardWFoxD (2005) Probabilistic Robotics. Cambridge, MA: The MIT Press.
62.
TriggsBMcLauchlanPHartleyRFitzgibbonA (1999) Bundle adjustment—a modern synthesis. In TriggsWZissermanASzeliskiR (eds), Vision Algorithms: Theory and Practice (Lecture Notes in Computer Science, vol. 1883). Berlin: Springer-Verlag, pp. 298–375.
63.
WangZ (2007) Exactly Sparse Information Filters for Simultaneous Localization and Mapping. PhD thesis, The University of Technology, Sydney.
Supplementary Material
Please find the following supplemental material available below.
For Open Access articles published under a Creative Commons License, all supplemental material carries the same license as the article it is associated with.
For non-Open Access articles published, all supplemental material carries a non-exclusive license, and permission requests for re-use of supplemental material or any part of supplemental material shall be sent directly to the copyright owner as specified in the copyright notice associated with the article.
