Duality-based verification techniques for 2D SLAM

While iterative optimization techniques for Simultaneous Localization and Mapping (SLAM) are now very efficient and widely used, none of them can guarantee global convergence to the maximum likelihood estimate. Local convergence usually implies artifacts in map reconstruction and large localization errors, hence it is very undesirable for applications in which accuracy and safety are of paramount importance. We provide a technique to verify if a given 2D SLAM solution is globally optimal. The insight is that, while computing the optimal solution is hard in general, duality theory provides tools to compute tight bounds on the optimal cost, via convex programming. These bounds can be used to evaluate the quality of a SLAM solution, hence providing a “sanity check” for state-of-the-art incremental and batch solvers. Experimental results show that our technique successfully identifies wrong estimates (i.e., local minima) in large-scale SLAM scenarios. This work, together with [1], represents a step towards the objective of having SLAM techniques with guaranteed performance, that can be used in safety-critical applications.

[1]  Frank Dellaert,et al.  Factor graph based incremental smoothing in inertial navigation systems , 2012, 2012 15th International Conference on Information Fusion.

[2]  Stephen P. Boyd,et al.  Convex Optimization , 2004, Algorithms and Theory of Computation Handbook.

[3]  Luca Carlone,et al.  A convergence analysis for pose graph optimization via Gauss-Newton methods , 2013, 2013 IEEE International Conference on Robotics and Automation.

[4]  T. Tao,et al.  Honeycombs and sums of Hermitian matrices , 2000, math/0009048.

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

[6]  Danny C. Sorensen,et al.  Minimization of a Large-Scale Quadratic FunctionSubject to a Spherical Constraint , 1997, SIAM J. Optim..

[7]  James M. Taylor.  Eigenvalues for Sums of Hermitian Matrices , 2015 .

[8]  Heng Wang,et al.  On the Structure of Nonlinearities in Pose Graph SLAM , 2012, Robotics: Science and Systems.

[9]  J. R. Bar-On,et al.  Global Optimization of a Quadratic Functional with Quadratic Equality Constraints, Part 2 , 1997 .

[10]  Prabir Barooah,et al.  Error growth in position estimation from noisy relative pose measurements , 2013, Robotics Auton. Syst..

[11]  Gamini Dissanayake,et al.  How far is SLAM from a linear least squares problem? , 2010, 2010 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[12]  Gamini Dissanayake,et al.  Novel insights into the impact of graph structure on SLAM , 2014, 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems.

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

[14]  Basilio Bona,et al.  A Linear Approximation for Graph-based Simultaneous Localization and Mapping , 2011, Robotics: Science and Systems.

[15]  Carl D. Meyer,et al.  Matrix Analysis and Applied Linear Algebra , 2000 .

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

[17]  Luca Carlone,et al.  From Angular Manifolds to the Integer Lattice: Guaranteed Orientation Estimation With Application to Pose Graph Optimization , 2012, IEEE Transactions on Robotics.

[18]  Frank Dellaert,et al.  Linear 2D localization and mapping for single and multiple robot scenarios , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[19]  Heng Wang,et al.  On the number of local minima to the point feature based SLAM problem , 2012, 2012 IEEE International Conference on Robotics and Automation.

[20]  William Gropp,et al.  Optimization environments and the NEOS server , 1997 .

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

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

[23]  Frank Dellaert,et al.  Selecting good measurements via ℓ1 relaxation: A convex approach for robust estimation over graphs , 2014, 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[24]  René Vidal,et al.  Intrinsic consensus on SO(3) with almost-global convergence , 2012, 2012 IEEE 51st IEEE Conference on Decision and Control (CDC).

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

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

[27]  Johan Fredriksson,et al.  Simultaneous Multiple Rotation Averaging Using Lagrangian Duality , 2012, ACCV.

[28]  Hongdong Li,et al.  Rotation Averaging , 2013, International Journal of Computer Vision.

[29]  J. R. Bar-on,et al.  Global optimization of a quadratic functional with quadratic equality constraints , 1994 .

[30]  Jorge J. Moré,et al.  The NEOS Server , 1998 .

[31]  John J. Leonard,et al.  RISE: An Incremental Trust-Region Method for Robust Online Sparse Least-Squares Estimation , 2014, IEEE Transactions on Robotics.