MH-iSAM2: Multi-hypothesis iSAM using Bayes Tree and Hypo-tree

A novel nonlinear incremental optimization algorithm MH-iSAM2 is developed to handle ambiguity in simultaneous localization and mapping (SLAM) problems in a multi-hypothesis fashion. It can output multiple possible solutions for each variable according to the ambiguous inputs, which is expected to greatly enhance the robustness of autonomous systems as a whole. The algorithm consists of two data structures: an extension of the original Bayes tree that allows efficient multi-hypothesis inference, and a Hypo-tree that is designed to explicitly track and associate the hypotheses of each variable as well as all the inference processes for optimization. With our proposed hypothesis pruning strategy, MH-iSAM2 enables fast optimization and avoids the exponential growth of hypotheses. We evaluate MH-iSAM2 using both simulated datasets and real-world experiments, demonstrating its improvements on the robustness and accuracy of SLAM systems.

[1]  Juan D. Tardós,et al.  Data association in stochastic mapping using the joint compatibility test , 2001, IEEE Trans. Robotics Autom..

[2]  John J. Leonard,et al.  A nonparametric belief solution to the Bayes tree , 2016, 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[3]  Niko Sünderhauf,et al.  Towards a robust back-end for pose graph SLAM , 2012, 2012 IEEE International Conference on Robotics and Automation.

[4]  Stergios I. Roumeliotis,et al.  Analytically-selected multi-hypothesis incremental MAP estimation , 2013, 2013 IEEE International Conference on Acoustics, Speech and Signal Processing.

[5]  Sebastian Thrun,et al.  Simultaneous localization and mapping with unknown data association using FastSLAM , 2003, 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422).

[6]  Guofeng Zhang,et al.  Keyframe-based dense planar SLAM , 2017, 2017 IEEE International Conference on Robotics and Automation (ICRA).

[7]  Panos E. Trahanias,et al.  Using multi-hypothesis mapping to close loops in complex cyclic environments , 2006, Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006..

[8]  Frank Dellaert,et al.  Factor Graphs for Robot Perception , 2017, Found. Trends Robotics.

[9]  Peter Protzel,et al.  Robust Sensor Fusion with Self-Tuning Mixture Models , 2018, 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[10]  Wolfram Burgard,et al.  G2o: A general framework for graph optimization , 2011, 2011 IEEE International Conference on Robotics and Automation.

[11]  Frank Dellaert,et al.  iSAM2: Incremental smoothing and mapping using the Bayes tree , 2012, Int. J. Robotics Res..

[12]  Robert C. Bolles,et al.  Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography , 1981, CACM.

[13]  Javier González,et al.  Toward a Unified Bayesian Approach to Hybrid Metric--Topological SLAM , 2008, IEEE Transactions on Robotics.

[14]  Andreas Birk,et al.  Generalized graph SLAM: Solving local and global ambiguities through multimodal and hyperedge constraints , 2016, Int. J. Robotics Res..

[15]  Niko Sünderhauf,et al.  Switchable constraints for robust pose graph SLAM , 2012, 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[16]  Howie Choset,et al.  A multi-hypothesis topological SLAM approach for loop closing on edge-ordered graphs , 2009, 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems.

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

[18]  Yasir Latif,et al.  Robust loop closing over time for pose graph SLAM , 2013, Int. J. Robotics Res..

[19]  Jonathan P. How,et al.  SLAM with objects using a nonparametric pose graph , 2016, 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

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