How-to Augmented Lagrangian on Factor Graphs

TL;DR

This paper extends factor graph solvers to handle constrained optimization problems using an Augmented Lagrangian approach, demonstrated across robotics, control, and computer vision applications.

Contribution

It introduces a novel method to incorporate Augmented Lagrangian techniques into factor graphs, enabling constrained optimization in diverse robotics problems.

Findings

Effective in pose estimation, rotation synchronization, and MPC.

Favorable comparison with domain-specific approaches.

Implemented in C++ and ROS for practical use.

Abstract

Factor graphs are a very powerful graphical representation, used to model many problems in robotics. They are widely spread in the areas of Simultaneous Localization and Mapping (SLAM), computer vision, and localization. In this paper we describe an approach to fill the gap with other areas, such as optimal control, by presenting an extension of Factor Graph Solvers to constrained optimization. The core idea of our method is to encapsulate the Augmented Lagrangian (AL) method in factors of the graph that can be integrated straightforwardly in existing factor graph solvers. We show the generality of our approach by addressing three applications, arising from different areas: pose estimation, rotation synchronization and Model Predictive Control (MPC) of a pseudo-omnidirectional platform. We implemented our approach using C++ and ROS. Besides the generality of the approach, application results show that we can favorably compare against domain specific approaches.