How-to Augmented Lagrangian on Factor Graphs

TL;DR

The core idea of the method is to encapsulate the Augmented Lagrangian (AL) method in factors that can be integrated straightforwardly in existing factor graph solvers, and application results show that it can favorably compare against domain specific approaches.

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.

Figures (11)

• Figure 1: Three application studies: (a) pose estimation; (b) rotation synchronization; (c) of a pseudo-onidirectional platform; the robot is traveling across three goals, with desired orientation represented by the arrows.
• Figure 2: Factor graph modeling the pose estimation problem subject to kinematics which constrains the pose on a circumference with radial orientation.
• Figure 3: Representation of the constrained estimation problem. By adding the constraint Eq. (\ref{['eq:circumference-constraint']}) to the optimization problem, $\mathbf{t}_{\mathrm{CONSTRAINED}}$ is closer to Ground Truth $[1,0,0]$ compared to $\mathbf{t}_{\mathrm{FREE}}$.
• Figure 4: Probability distribution of linear and rotational error of the estimate with and without the constraint of Eq. (\ref{['eq:circumference-constraint']}). Each test corresponds to a different $\mathbf{z}_{\mathrm{GPS}}$ extracted from the normal distribution $\mathcal{N}([1,0,0]; \mathbf{\Omega}_{\mathrm{GPS}})$, with $\theta_0 = 0.5\mathrm{rad}$.
• Figure 5: Factor graph modeling the rotation synchronization problem, $n=4$.