Incremental cycle bases for cycle-based pose graph optimization

TL;DR

This paper introduces incremental algorithms for constructing sparse cycle bases in pose graph optimization, enabling efficient and flexible multi-agent SLAM with various measurement types.

Contribution

It presents novel incremental cycle basis algorithms, an approximation method for multi-agent graphs, and extends relative pose parameterization to lower-degree-of-freedom measurements.

Findings

Incremental cycle basis construction improves sparsity and efficiency.

The algorithms outperform traditional minimum cycle basis methods in benchmarks.

Flexible measurement modeling enhances pose graph optimization capabilities.

Abstract

Pose graph optimization is a special case of the simultaneous localization and mapping problem where the only variables to be estimated are pose variables and the only measurements are inter-pose constraints. The vast majority of pose graph optimization techniques are vertex based (variables are robot poses), but recent work has parameterized the pose graph optimization problem in a relative fashion (variables are the transformations between poses) that utilizes a minimum cycle basis to maximize the sparsity of the problem. We explore the construction of a cycle basis in an incremental manner while maximizing the sparsity. We validate an algorithm that constructs a sparse cycle basis incrementally and compare its performance with a minimum cycle basis. Additionally, we present an algorithm to approximate the minimum cycle basis of two graphs that are sparsely connected as is common in multi-agent scenarios. Lastly, the relative parameterization of pose graph optimization has been limited to using rigid body transforms on SE(2) or SE(3) as the constraints between poses. We introduce a methodology to allow for the use of lower-degree-of-freedom measurements in the relative pose graph optimization problem. We provide extensive validation of our algorithms on standard benchmarks, simulated datasets, and custom hardware.