COP-SLAM: Closed-Form Online Pose-Chain Optimization for Visual SLAM

In this paper, we analyze and extend the recently proposed closed-form online pose-chain simultaneous localization and mapping (SLAM) algorithm. Pose-chains are a specific type of extremely sparse pose-graphs and a product of contemporary SLAM front-ends, which perform accurate visual odometry and reliable appearance-based loop detection. They are relevant for challenging robotic applications in large-scale 3-D environments for which frequent loop detection is not desired or not possible. Closed-form online pose-chain SLAM efficiently and accurately optimizes pose-chains by exploiting their Lie group structure. The convergence and optimality properties of this solution are discussed in detail and are compared against state-of-the-art iterative methods. We also provide a novel solution space, that of similarity transforms, which has not been considered earlier for the proposed algorithm. This allows for closed-form optimization of pose-chains that exhibit scale drift, which is important to monocular SLAM systems. On the basis of extensive experiments, specifically targeting 3-D pose-chains and using a total of 60 km of challenging binocular and monocular data, it is shown that the accuracy obtained by closed-form online pose-chain SLAM is comparable with that of state-of-the-art iterative methods, while the time it needs to compute its solution is orders of magnitudes lower. This novel SLAM technique thereby is relevant to a broad range of robotic applications and computational platforms.

[1]  Supun Samarasekera,et al.  Ten-fold Improvement in Visual Odometry Using Landmark Matching , 2007, 2007 IEEE 11th International Conference on Computer Vision.

[2]  Brett Browning,et al.  Closed-form Online Pose-chain SLAM , 2013, 2013 IEEE International Conference on Robotics and Automation.

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

[4]  F. Park.  Distance Metrics on the Rigid-Body Motions with Applications to Mechanism Design , 1995 .

[5]  Xavier Pennec,et al.  Intrinsic Statistics on Riemannian Manifolds: Basic Tools for Geometric Measurements , 2006, Journal of Mathematical Imaging and Vision.

[6]  Julius Ziegler,et al.  StereoScan: Dense 3d reconstruction in real-time , 2011, 2011 IEEE Intelligent Vehicles Symposium (IV).

[7]  Jan-Michael Frahm,et al.  USAC: A Universal Framework for Random Sample Consensus , 2013, IEEE Transactions on Pattern Analysis and Machine Intelligence.

[8]  Brett Browning,et al.  Bias compensation in visual odometry , 2012, 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[9]  James R. Bergen,et al.  Visual odometry , 2004, Proceedings of the 2004 IEEE Computer Society Conference on Computer Vision and Pattern Recognition, 2004. CVPR 2004..

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

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

[12]  Michael Bosse,et al.  Place Recognition Using Regional Point Descriptors for 3D Mapping , 2009, FSR.

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

[14]  James J. Little,et al.  /spl sigma/SLAM: stereo vision SLAM using the Rao-Blackwellised particle filter and a novel mixture proposal distribution , 2006, Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006..

[15]  Kurt Konolige,et al.  FrameSLAM: From Bundle Adjustment to Real-Time Visual Mapping , 2008, IEEE Transactions on Robotics.

[16]  Basilio Bona,et al.  A fast and accurate approximation for planar pose graph optimization , 2014, Int. J. Robotics Res..

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

[18]  Paul Newman,et al.  Appearance-only SLAM at large scale with FAB-MAP 2.0 , 2011, Int. J. Robotics Res..

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

[20]  Klamer Schutte,et al.  Efficient trajectory bending with applications to loop closure , 2010, 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[21]  Hanumant Singh,et al.  Exactly Sparse Delayed-State Filters for View-Based SLAM , 2006, IEEE Transactions on Robotics.

[22]  Andreas Geiger,et al.  Vision meets robotics: The KITTI dataset , 2013, Int. J. Robotics Res..

[23]  Gamini Dissanayake,et al.  Linear SLAM: A linear solution to the feature-based and pose graph SLAM based on submap joining , 2013, 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[24]  Paul Newman,et al.  Outdoor SLAM using visual appearance and laser ranging , 2006, Proceedings 2006 IEEE International Conference on Robotics and Automation, 2006. ICRA 2006..

[25]  Frans C. A. Groen,et al.  Accurate and robust ego-motion estimation using expectation maximization , 2008, 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[26]  Brett Browning,et al.  Orientation only loop-closing with closed-form trajectory bending , 2012, 2012 IEEE International Conference on Robotics and Automation.

[27]  Frank Dellaert,et al.  Generalized subgraph preconditioners for large-scale bundle adjustment , 2011, 2011 International Conference on Computer Vision.

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

[29]  Gijs Dubbelman,et al.  Intrinsic statistical techniques for robust pose estimation , 2011 .

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

[31]  Cyrill Stachniss,et al.  On measuring the accuracy of SLAM algorithms , 2009, Auton. Robots.

[32]  Hauke Strasdat,et al.  Scale Drift-Aware Large Scale Monocular SLAM , 2010, Robotics: Science and Systems.

[33]  Winston Churchill,et al.  The New College Vision and Laser Data Set , 2009, Int. J. Robotics Res..

[34]  Hauke Strasdat,et al.  Real-time monocular SLAM: Why filter? , 2010, 2010 IEEE International Conference on Robotics and Automation.

[35]  Yasir Latif,et al.  Robust Loop Closing Over Time , 2012, Robotics: Science and Systems.

[36]  François Michaud,et al.  Memory management for real-time appearance-based loop closure detection , 2011, 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems.

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

[38]  José Neira,et al.  SLAM in O(log n) with the Combined Kalman-Information Filter , 2010, Robotics Auton. Syst..

[39]  José Neira,et al.  SLAM in O(log n) with the Combined Kalman - Information filter , 2009, 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[40]  Ian D. Reid,et al.  RSLAM: A System for Large-Scale Mapping in Constant-Time Using Stereo , 2011, International Journal of Computer Vision.

[41]  Olivier Stasse,et al.  MonoSLAM: Real-Time Single Camera SLAM , 2007, IEEE Transactions on Pattern Analysis and Machine Intelligence.

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

[43]  Javier Civera,et al.  Camera self-calibration for sequential Bayesian structure from motion , 2009, 2009 IEEE International Conference on Robotics and Automation.

[44]  Yann LeCun,et al.  Hybrid hessians for flexible optimization of pose graphs , 2010, 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[45]  Oliver Birbach,et al.  Rapid development of manifold-based graph optimization systems for multi-sensor calibration and SLAM , 2011, 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems.