Incremental Cycle Bases for Cycle-Based Pose Graph Optimization

Post 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.

Automated summary

This paper investigates the problems of cycle basis construction and sparsity maximization in relative pose graph optimization, and validates an algorithm's performance compared to the minimum cycle basis. Furthermore, a new methodology is introduced to enable the use of lower-degree-of-freedom measurements in the relative pose graph optimization problem. Extensive validation of the algorithms is conducted on standard benchmarks, simulated datasets, and custom hardware.