How-to Augmented Lagrangian on Factor Graphs

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. However, the physics of many real-world problems is better modeled through constraints, e.g., estimation in the presence of inconsistent measurements, or optimal control. Constraints handling is hard because the solution cannot be found by following the gradient descent direction as done by traditional factor graph solvers. The core idea of our method is to encapsulate the Augmented Lagrangian (AL) method in factors that can be integrated straightforwardly in existing factor graph solvers. Besides being a tool to unify different robotics areas, the modularity of factor graphs allows to easily combine multiple objectives and effectively exploiting the problem structure for efficiency. 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. Application results show that we can favorably compare against domain specific approaches.