Kimera-RPGO: pose graph optimization with outlier rejection for SLAM

A backend for graph optimization, not a SLAM application

Kimera-RPGO is a library for pose graph optimization that rejects outliers, and the README is precise about the scope. It can be used as an optimization backend, with the default solver being LM, Levenberg-Marquardt, or as a standalone experimental tool. The latter comes with a g2o reader in the examples directory, alongside a trajectory generator and an incremental variant of the g2o reader. In practice, the library is the optimization layer underneath the Kimera library, the open-source library for real-time metric-semantic localization and mapping by Rosinol, Abate, Chang and Carlone, published at ICRA 2020. The audience is a robotics researcher or engineer who already has a factor graph of odometry and loop closure measurements and needs the graph to be optimised consistently while the bad measurements are thrown out.

GTSAM is a dependency, and the EXPMAP flags are not optional

The dependency section names GTSAM, the Georgia Tech Smoothing and Mapping library, and gives a tested commit, 686e16aaae26c9a4f23d4af7f2d4a504125ec9c3. There is a parenthetical warning that the BUILD_WITH_MARCH_NATIVE flag caused some problems on some machines. The build instructions are the part worth reading carefully, because the flags are not defaults you can skip. The reason EXPMAP is needed is the correct calculation of Jacobians. Enabling it and defining SLOW_BUT_CORRECT_BETWEENFACTOR before any include of gtsam/slam/BetweenFactor.h are both important, and without them the default is some identity approximations for rotations, which works for some cases but fails for cases with manual loop closures or artifacts. The README also notes that sudo make check inside GTSAM will partially fail, because some unit tests assume the EXPMAP flags are off, so a red test run there is expected rather than a sign of a broken build.

Building GTSAM and then Kimera-RPGO

GTSAM is cloned and built with CMake, with the two EXPMAP options passed on the configure line:

git clone [email protected]:borglab/gtsam.git

cd gtsam
mkdir build
cd build
cmake .. -DGTSAM_POSE3_EXPMAP=ON -DGTSAM_ROT3_EXPMAP=ON
sudo make install

Kimera-RPGO itself then follows the same shape, and the build is a plain CMake project with a Makefile, an include directory, a src directory, a tests directory and a cmake directory, plus a package.xml for ROS users:

git clone [email protected]:MIT-SPARK/Kimera-RPGO.git
cd Kimera-RPGO
mkdir build
cd build
cmake ..
make

The structure tells you what you are getting. The library is header and source, with examples and tests alongside, and the .linterconfig.yaml and .clang-format files mean the project expects a specific style and a linter pass over the tree. The package.xml is a hint that the intended deployment is inside a robotics stack that already uses ROS packaging.

Setting up the solver: thresholds, verbosity and the choice of GN

The usage section shows a small setup sequence that sets most of the behaviour. RobustSolverParams is created, the PCM parameters are set for 3D with a pair of thresholds and a verbosity level, and the solver is constructed with KimeraRPGO::make_unique:

// Set up
// set up KimeraRPGO solver
RobustSolverParams params;
params.setPcm3DParams(0.0, 10.0, Verbosity::QUIET);
// Verbosity levels are QUIET, UPDATE, and, VERBOSE in order of increasing number of messages (the default is UPDATE)
// For 2D params.setPcm2DParams(0.0, 10.0); Have been tested
// If threshold is set to < 0, either odom check or loop consistency check disabled

// To use GaussNewton instead of LM: params.solver = Solver::GN;

std::unique_ptr<RobustSolver> pgo = KimeraRPGO::make_unique<RobustSolver>(params);

The two numbers in setPcm3DParams are the translation and rotation thresholds for the consistency check, and setting a threshold below zero disables either the odometry check or the loop consistency check, which is a useful switch when you want to see the raw optimisation without the rejection stage. Verbosity has three levels, QUIET, UPDATE and VERBOSE, and UPDATE is the default.

Running the solver on a g2o file as a standalone experiment

The standalone mode is a g2o reader that takes the file, two thresholds and optional output arguments, and the README gives both the two dimensional and the three dimensional form:

# for 2D:
./RpgoReadG2o 2d <g2o-file> <odom-check-threshold> <pcm-threshold> <optional:folder-to-save-g2o> <optional:v to toggle verbosity>

# for 3D
./RpgoReadG2o 3d <g2o-file> <odom-check-threshold> <pcm-threshold> <optional:folder-to-save-g2o> <optional:v to toggle verbosity>

A worked invocation is given as well, running the 3D form on a file with a threshold of 1.0 for both checks, an output folder and the verbosity flag. The two thresholds are the same pair used in the library parameters, one for the odometry check and one for the PCM check, which makes the standalone tool a reasonable way to tune those values against a known dataset before wiring them into a live system.

Outlier rejection, and the maximum clique finder it borrows from

The rejection stage is what separates this from a plain graph optimizer, and the README credits it with care. The related publications section names the outlier rejection work it builds on, the pairwise consistent measurement set maximization approach of Mangelson, Dominic, Eustice and Vasudevan for multi-robot map merging that resists bad measurements, published at ICRA 2018. That method is implemented with a maximum clique finder, the one from the Northwestern University project page, used with slight adaptations. The references behind that implementation are the fast maximum clique algorithms of Pattabiraman and colleagues, one for massive graphs and one for massive sparse graphs, both cited in the README. The practical consequence for a user is that the rejection is combinatorial rather than statistical, which is why the thresholds matter and why the standalone tool exists, since the clique search is where the parameters are tuned.

Loading, merging and turning rejection off

The example section shows the full API surface, and it is short enough to read as a summary. The solver is created from params, then pgo->update takes a new factor and new values for the normal incremental use. For bulk work there is loadGraph with a prior factor or a key to load only the first key, and addGraph to merge in another robot's graph with a between factor, the case the multi-robot citation covers. Finally there is the escape hatch, setNoRejection on the params, which sets the solver up with the rejection stage turned off entirely, and the same construction follows. One detail in the example is worth noting for anyone using it with landmark factors, the specialSymbols vector with the characters l and u for landmarks, which tells the solver which symbols are not poses.

Editorial conclusion

Kimera-RPGO fits a robotics team that already runs the Kimera stack and needs the graph optimization backend with outlier rejection rather than a bare least squares solver, or a researcher who wants to test the maximum clique based rejection on a g2o dataset. It does not fit a team starting a new SLAM system from scratch, since it is a component of a larger library rather than an application, and it is not a general graph optimizer. Before using it, build GTSAM with GTSAM_POSE3_EXPMAP and GTSAM_ROT3_EXPMAP turned on and define SLOW_BUT_CORRECT_BETWEENFACTOR, and set the two thresholds explicitly for your problem rather than relying on the defaults.

Frequently asked questions

Official sources

1. Issues
2. License: BSD-2-Clause
3. MIT-SPARK/Kimera-RPGO on GitHub
4. README