A Factor Graph Approach To Constrained Optimization

Abstract

Several problems in robotics can be solved using constrained optimization. For example, solutions in areas like control and planning frequently use it. Meanwhile, the Georgia Tech Smoothing and Mapping (GTSAM) toolbox provides a straight forward way to represent sparse least-square optimization problems as factor graphs. Factor graphs, are a popular graphical model to represent a factorization of a probability distribution allowing for efficient computations. This paper demonstrates the use of the GTSAM and factor graphs to solve linear and quadratic constrained optimization programs using the active set method. It also includes an implementation of a line search method for sequential quadratic programming that can solve nonlinear equality constrained problems. The result is a constrained optimization framework that allows the user to think of optimization problems as solving a series of factor graphs and is open-source.