Abstract:We present the first algorithm to efficiently compute certifiably optimal solutions to range-aided simultaneous localization and mapping (RA-SLAM) problems. Robotic navigation systems increasingly incorporate point-to-point ranging sensors, leading to state estimation problems in the form of RA-SLAM. However, the RA-SLAM problem is significantly more difficult to solve than traditional pose-graph SLAM: Ranging sensor models introduce nonconvexity and single range measurements do not uniquely determine the transform between the involved sensors. As a result, RA-SLAM inference is sensitive to initial estimates yet lacks reliable initialization techniques. Our approach, certifiably correct RA-SLAM (CORA), leverages a novel quadratically constrained quadratic programming formulation of RA-SLAM to relax the RA-SLAM problem to a semidefinite program (SDP). CORA solves the SDP efficiently using the Riemannian Staircase methodology; the SDP solution provides both: 1) a lower bound on the RA-SLAM problem's optimal value and 2) an approximate solution of the RA-SLAM problem, which can be subsequently refined using local optimization. CORA applies to problems with arbitrary pose-pose, pose-landmark, and ranging measurements and, due to using convex relaxation, is insensitive to initialization. We evaluate CORA on several real-world problems. In contrast to state-of-the-art approaches, CORA is able to obtain high-quality solutions on all problems despite being initialized with random values. In addition, we study the tightness of the SDP relaxation with respect to important problem parameters: The number of: 1) robots; 2) landmarks; and 3) range measurements. These experiments demonstrate that the SDP relaxation is often tight and reveal relationships between graph connectivity and the tightness of the SDP relaxation.

On Semidefinite Relaxations for Matrix-Weighted State-Estimation Problems in Robotics

On Semidefinite Relaxations for Matrix-Weighted State-Estimation Problems in Robotics

Toward Globally Optimal State Estimation Using Automatically Tightened Semidefinite Relaxations

New Trend in Back-End Techniques of Visual SLAM: from Local Iterative Solvers to Robust Global Optimization

Certifiably optimal rotation and pose estimation based on the Cayley map

Bias Reduced Semidefinite Relaxation Method for 3-D Rigid Body Localization Using AOA

Semidefinite relaxations for certifying robustness to adversarial examples

Tightened Convex Relaxations for Neural Network Robustness Certification

An Analysis of Constraint-Relaxation in PDE-Based Inverse Problems

Scale jump-aware pose graph relaxation for monocular SLAM with re-initializations

Towards Optimal Branching of Linear and Semidefinite Relaxations for Neural Network Robustness Certification

Instance-specific linear relaxations of semidefinite optimization problems

Tight Certification of Adversarially Trained Neural Networks via Nonconvex Low-Rank Semidefinite Relaxations

Certifiably Correct Range-Aided SLAM

Noisy Matrix Completion: Understanding Statistical Guarantees for Convex Relaxation via Nonconvex Optimization

Noisy matrix decomposition via convex relaxation: Optimal rates in high dimensions

Safe and Smooth: Certified Continuous-Time Range-Only Localization

Tight Neural Network Verification via Semidefinite Relaxations and Linear Reformulations