Handling Constrained Optimization in Factor Graphs for Autonomous Navigation

TL;DR

This paper introduces a novel method for incorporating constraints into factor graph optimization using Lagrange multipliers, enabling efficient and constrained autonomous navigation solutions that outperform standard solvers in real-world tests.

Contribution

The paper presents a new approach to model constraints in factor graphs with Lagrange multipliers, allowing constrained optimization in autonomous navigation tasks.

Findings

Outperforms IPOPT in runtime for navigation problems

Achieves similar solution quality to standard solvers

Enables modeling of both control and localization within factor graphs

Abstract

Factor graphs are graphical models used to represent a wide variety of problems across robotics, such as Structure from Motion (SfM), Simultaneous Localization and Mapping (SLAM) and calibration. Typically, at their core, they have an optimization problem whose terms only depend on a small subset of variables. Factor graph solvers exploit the locality of problems to drastically reduce the computational time of the Iterative Least-Squares (ILS) methodology. Although extremely powerful, their application is usually limited to unconstrained problems. In this paper, we model constraints over variables within factor graphs by introducing a factor graph version of the method of Lagrange Multipliers. We show the potential of our method by presenting a full navigation stack based on factor graphs. Differently from standard navigation stacks, we can model both optimal control for local planning and localization with factor graphs, and solve the two problems using the standard ILS methodology. We validate our approach in real-world autonomous navigation scenarios, comparing it with the de facto standard navigation stack implemented in ROS. Comparative experiments show that for the application at hand our system outperforms the standard nonlinear programming solver Interior-Point Optimizer (IPOPT) in runtime, while achieving similar solutions.