Robust optimization of factor graphs by using condensed measurements

Popular problems in robotics and computer vision like simultaneous localization and mapping (SLAM) or structure from motion (SfM) require to solve a least-squares problem that can be effectively represented by factor graphs. The chance to find the global minimum of such problems depends on both the initial guess and the non-linearity of the sensor models. In this paper we propose an approach to determine an approximation of the original problem that has a larger convergence basin. To this end, we employ a divide-and-conquer approach that exploits the structure of the factor graph. Our approach has been validated on real-world and simulated experiments and is able to succeed in finding the global minimum in situations where other state-of-the-art methods fail.

[1]  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..

[2]  Cyrill Stachniss,et al.  Hierarchical optimization on manifolds for online 2D and 3D mapping , 2010, 2010 IEEE International Conference on Robotics and Automation.

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

[4]  Frank Dellaert,et al.  Multi-level submap based SLAM using nested dissection , 2010, 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[5]  Richard Szeliski,et al.  Pushing the Envelope of Modern Methods for Bundle Adjustment , 2012, IEEE Trans. Pattern Anal. Mach. Intell..

[6]  Kurt Konolige,et al.  Double window optimisation for constant time visual SLAM , 2011, 2011 International Conference on Computer Vision.

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

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

[9]  Udo Frese,et al.  Integrating generic sensor fusion algorithms with sound state representations through encapsulation of manifolds , 2011, Inf. Fusion.

[10]  Kurt Konolige,et al.  Sparse Sparse Bundle Adjustment , 2010, BMVC.

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

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

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

[14]  Wolfram Burgard,et al.  Nonlinear Constraint Network Optimization for Efficient Map Learning , 2009, IEEE Transactions on Intelligent Transportation Systems.

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

[16]  Nicholas Roy,et al.  A Linear Approximation for Graph-Based Simultaneous Localization and Mapping , 2012 .

[17]  Jan-Michael Frahm,et al.  Online environment mapping , 2011, CVPR 2011.

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

[19]  Simon J. Julier,et al.  The scaled unscented transformation , 2002, Proceedings of the 2002 American Control Conference (IEEE Cat. No.CH37301).

[20]  Sebastian Thrun,et al.  Large-Scale Robotic 3-D Mapping of Urban Structures , 2004, ISER.

[21]  Frank Dellaert,et al.  Incremental smoothing and mapping , 2008 .

[22]  Wolfram Burgard,et al.  Efficient Sparse Pose Adjustment for 2D mapping , 2010, 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems.

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

[24]  Timothy A. Davis,et al.  Direct methods for sparse linear systems , 2006, Fundamentals of algorithms.

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