SCORE: Second Order Conic Initialization for RA-SLAM

Code to solve a second-order cone program to initialize a local-search solver for the range-aided SLAM problem. The SOCP is a convex relaxation of the original problem.

Check out the extended version of our paper or our short video summary.

Results

We show the key results from our paper, comparing SCORE to a range of other initialization strategies.

Real-World AUV Experiments

• SCORE: our method, using a second-order cone program for initialization
• Odom: initializing with robot odometry

Simulated Multi-Robot Experiments

• SCORE: our method, using a second-order cone program for initialization
• SCORE Init: the estimate returned by SCORE (before refining via local-search)
• Odom-R: initializing with robot odometry, randomizing the first pose of each robot
• Odom-P: initializing with robot odometry, initializing with the true first pose for each robot
• GT-Init: initializing with the ground-truth values (when available)

Simulated Single-Robot Experiments

• SCORE: our method, using a second-order cone program for initialization
• Odom: initializing with robot odometry
• GT-Init: initializing with the ground-truth values (when available)

Usage

Feel free to look inside our /examples directory!

Dependencies

PyFactorGraph (required)

Link to the repo

This holds all of the measurements/variables to define our RA-SLAM problem. This is a custom library developed in the Marine Robotics Group at MIT to interface with a broader range of SLAM file types (e.g. g2o). You can install directly from source via:

cd ~/<repo_parent_directory>/PyFactorGraph
pip install .

GTSAM (optional)

We used GTSAM to refine our initial estimates provided by SCORE in the experiments in our paper.

pip install gtsam