by Zhenxin (Jason) Xu, Nikolas R. Sanderson, David M. Rosen. We are affiliated with the Robust Autonomy Lab at Northeastern University.
Factor graphs are the dominant paradigm for modeling state estimation tasks in mobile robotics, as they afford both a convenient modular modeling language and fast, scalable inference algorithms. However, most state-of-the-art factor graph inference approaches rely on local optimization, which makes them susceptible to converging to incorrect estimates. Recent work has led to the design of novel certifiable optimization algorithms capable of efficiently recovering verifiably globally optimal estimates in practice. However, Despite these advantages, the widespread adoption of certifiable estimation methods has been limited by the extensive manual effort required to custom-design appropriate relaxations and efficient optimization algorithms. To address these challenges, in this paper we present a method that leverages the same factor graph and local optimization framework widely used in robotics and computer vision to design and deploy a broad range of certifiable estimators. We describe how to implement lifted versions of the variable and factor types typically encountered in robotic mapping and localization problems. The result is a set of certifiable factors that enables practitioners to develop and deploy globally optimal estimators with the same ease as conventional local methods. Experimental results validate our approach, demonstrating global optimality comparable to that achieved by state-of-the-art certifiable solvers.
Presented at ICRA 2025 Atlanta Georgia in workshop "Robots in the Wild".
The C++ implementation can be built and exported as a CMake project.
The following installation instructions have been verified on Ubuntu 22.04:
Step 1: Install dependencies
sudo apt-get install liblapack-dev libblas-dev libsuitesparse-dev
Eigen (Make sure using the same version o external as GTSAM's to avoid conflicts, i.e., you can enable -DGTSAM_USE_SYSTEM_EIGEN=ON when compile GTSAM)
GTSAM (tested on release/4.3a0, should also compatible with a series of versions that after Boost Removal, we use OptionalMatrixType but not boost optional matrix for customer factors)
(If you have multiple GTSAM versions in your machine, and do not want to destroy the environment. The easiest thing to do may be just specify the path to your GTSAM compatible with our project by :
set(GTSAM_DIR "path to your /gtsam/build")
set(GTSAM_UNSTABLE_DIR "path to your /gtsam/build")
## Above your find_package(GTSAM REQUIRED) and :
set(GTSAMCMakeTools_DIR "path to your /gtsam/cmake")
## Above your find_package(GTSAMCMakeTools REQUIRED)
)
Step 2: Clone the repository
git clone git@github.com:NEU-RAL/CertifiableFactors.git
Step 3: Initialize Git submodules
cd CertifiableFactors
git submodule init
git submodule update
Step 4: Create build directory
mkdir build
Step 5: Configure build and generate Makefiles
cd build && cmake ..
Step 6: Build code
make -j
Step 7: Run the examples
./xxx/build/bin d p input_file.g2o output_file.csv
For example, when running a 2D example(d=2), and set the initial rank as p=d=2(Note that for any examples, p should be >=d):
./your_executable 2 2 input_file.g2o output_file.csv
Landmark-slam and range-aided example will be released soon.
We believe that certifiable factors offer a promising approach to bring certifiable estimation into practice. If you're interested—whether it's expanding to more certifiable examples or using certifiable estimation as the backend of your real-time state estimation system—we’d be excited to collaborate with you!
We use David M. Rosen's Optimization and Preconditioners for the eigenvalue computation of fast verification.