ecg2o: A Seamless Extension of g2o for Equality-Constrained Factor Graph Optimization

TL;DR

The paper addresses the challenge of incorporating equality constraints into factor-graph optimization to enable constrained control within robotics. It proposes a KKT-based Gauss-Newton method, implemented in a header-only ecg2o extension to the g2o library, and contrasts it with the Augmented Lagrangian approach. The key contributions include representing equality constraints as dedicated factor-graph edges with integrated Lagrange multipliers, a practical stopping criterion, and a velocity-tracking case study showing faster convergence and reduced tuning overhead. This work advances constrained optimization in robotic perception-control pipelines and provides open-source tooling to foster further research and application.

Abstract

Factor graph optimization serves as a fundamental framework for robotic perception, enabling applications such as pose estimation, simultaneous localization and mapping (SLAM), structure-from-motion (SfM), and situational awareness. Traditionally, these methods solve unconstrained least squares problems using algorithms such as Gauss-Newton and Levenberg-Marquardt. However, extending factor graphs with native support for equality constraints can improve solution accuracy and broaden their applicability, particularly in optimal control. In this paper, we propose a novel extension of factor graphs that seamlessly incorporates equality constraints without requiring additional optimization algorithms. Our approach maintains the efficiency and flexibility of existing second-order optimization techniques while ensuring constraint feasibility. To validate our method, we apply it to an optimal control problem for velocity tracking in autonomous vehicles and benchmark our results against state-of-the-art constraint handling techniques. Additionally, we introduce ecg2o, a header-only C++ library that extends the widely used g2o factor graph library by adding full support for equality-constrained optimization. This library, along with demonstrative examples and the optimal control problem, is available as open source at https://github.com/snt-arg/ecg2o

Figures (4)

• Figure 1: The factor graph for the optimal control problem. The red circles represent the cost terms as regular edges, while the blue circles illustrate the representation of equality constraints as regular edges. $k \in \{0, \cdots, N-1$}
• Figure 2: Optimal velocity trajectory obtained from the factor graph-based optimal control problem.
• Figure 3: Iteration number across different optimization scenarios and trajectory lengths.
• Figure 4: Computation time per iteration for different trajectory lengths using the Proposed Method and Augmented Lagrangian (AL) Method.