Direct Relative Edge Optimization, A Robust Alternative for Pose Graph Optimization

Abstract

Pose graph optimization is a common problem in robotics and associated fields. Most commonly, pose graph optimization is performed by finding the set of pose estimates which are the most likely for a given set of measurements. In some situations, arbitrarily large errors in pose graph initialization are unavoidable and can cause these pose-based methods to diverge or fail especially in cases where global inputs become available after some time after initialization. This letter details the parameterization of the classic pose graph problem in a relative context, optimizing directly over relative edge constraints between vertices in the pose graph and not on the poses themselves. Unlike previous literature on relative optimization, this letter details relative optimization over an entire pose graph, instead of a subset of edges, resulting in greater robustness to arbitrarily large errors than the classic pose-based or prior relative edge-based methods. Several small-scale simulation comparison studies, along with single and multi-agent hardware experiments, are presented. Results point to relative edge optimization as a strong candidate for solving real-world pose graph optimization problems that contain large heading propagation or initialization errors.