ecg2o: A Seamless Extension of g2o for Equality-Constrained Factor Graph Optimization*
Anas Abdelkarim, Holger Voos, Daniel Görges · 2025
Read ecg2o: A Seamless Extension of g2o for Equality-Constrained Factor Graph Optimization* by Anas Abdelkarim, Holger Voos, Daniel Görges (2025) in the Tower of Babel, a free online library of public-domain works preserved in clean, searchable text. No sign-up. The full text is served as a plain-text file at a predictable URL, where text is present: https://antarctic-labs.com/tower-of-babel/arxiv-2503-01311.txt.
Introduction
Equality bounds are wedded into a robot math library by Anas Abdelkarim, Holger Voos, and Daniel Görges in a 2025 arXiv paper. Factor graphs frame robot sight and map tasks as least-squares sums. Hard equality bounds can sharpen those sums, but old tools bolt them on. Their build threads bounds into the graph so stock fast solvers still run. Feasibility holds by build, not by patch.
A car speed tracking task benches the build against past bound handlers. The library, ecg2o, is a header-only add to the famed g2o. Code and cases are shared open.
Catalog record
| CREATOR | Anas Abdelkarim, Holger Voos, Daniel Görges |
|---|---|
| YEAR | 2025 |
| COLLECTION | PAPERS |
| FORMAT | TXT |
| FILE SIZE | 27 KB |
| VERSION | 1.0 |
| LICENSE | http://creativecommons.org/licenses/by/4.0/ |
| RIGHTS | CC BY |
| SOURCE | arXiv |
Source: https://arxiv.org/abs/2503-01311
Browse the full library · About the Tower of Babel
Full text
The opening of the work, from the library's hosted text:
ecg2o: A Seamless Extension of g2o for Equality-Constrained Factor Graph Optimization* Anas Abdelkarim, Holger Voos, Daniel Görges 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 . I Introduction Factor graphs are extensively used in robotic perception tasks for efficiently modeling large-scale probabilistic inference problems [ 1 ] . These methods use graph-based optimization techniques, such as weighted least squares, to address key problems like SLAM and situational awareness [ 2 , 3 ] . Typically, these optimization problems are addressed using second-order unconstrained algorithms, including Gauss-Newton [ 4 ] and Levenberg–Marquardt [ 5 ] . Several well-established libraries, such as GTSAM [ 6 ] , g2o [ 7 ] , and SRRG2 [ 8 ] , have been developed to provide robust back-end implementations of these optimization algorithms, along with user-friendly front-end interfaces tailored for robotic applications. Recent advancements in the literature have extended factor graph–based optimization beyond perception tasks to encompass optimal control applications. The primary challenge in this extension lies in handling constraints, as optimal control problems inherently involve constraints, unlike perception problems, which are typically unconstrained. However, using factor graphs for control has a significant advantage: it enables seamless integration between perception and control in robotics, creating a unified optimization framework. Additionally, this approach facilitates the reuse of established algorithms and computational efficiencies available in factor graph libraries [ 9 ] . In contrast, traditional optimization frameworks widely used in optimal control, such as CasADi [ 10 ] , AMPL [ 11 , 12 ] , and IPOPT [ 13 ] , although highly effective, do not naturally integrate with factor graph–based estimation frameworks. This disconnect can introduce computational inefficiencies and added complexity, particularly in robotic applications that demand a seamless integration of perception and control. It is important to highlight that, in optimal control problems, equality constraints define deterministic relationships between variables, such as system dynamics that strictly govern state transitions. In contrast, in SLAM, the motion and sensor models establish probabilistic relationships, where uncertainty is inherently modeled and optimized within the factor graph framework. Therefore, equality constraints in factor graphs enforce exact dependencies between variables, ensuring that the solution adheres to physically consistent state transitions, whereas probabilistic constraints model uncertainties and allow for flexible estimation I-A Related Work In our recent publication, we presented a comprehensive review of factor graphs in optimal control problems, along with various methods for handling constraints [ 9 ] . Here, we provide an overview of the key literature in this area. Chen and Zhang [ 14 ] introduced a general framework for applying Linear Quadratic Regulators (LQR) using factor graphs. In this framework, equality constraints are used to enforce system dynamics and are incorporated into the cost function as a weighted least squares term. By assigning a sufficiently large weighting matrix, these equality constraints effectively act as soft constraints, which are approximately satisfied as the weighting approaches infinity. Yang et al. [ 15 ] extended this approach by introducing additional equality constraints between variables within the LQR framework. Factor graph–based LQR has been applied to a variety of domains, including wireless mesh network control [ 16 ] , tactile estimation and extrinsic contact control [ 17 ] , and trajectory generation for graffiti robots [ 18 , 19 ] . Notably, these implementations have primarily relied on the GTSAM library to formulate and solve the optimization problems. Beyond LQR-based approaches, more advanced constrained optimization techniques have been explored for handling equality constraints, most notably the Augmented Lagrangian (AL) method [ 20 ] . The fundamental idea of AL is to introduce Lagrange multiplier terms and a quadratic penalty term for constraint violations into the cost function, thereby forming the Augmented Lagrangian function. The optimization process involves two nested loops: the inner loop minimizes the Augmented Lagrangian function while keeping the Lagrange multipliers fixed, while the outer loop updates the Lagrange multipliers and penalty parameters until a specified stopping criterion is met. The AL method has been implemented in GTSAM for improved state estimation [ 21 ] and has been applied in navigation and 3D manipulation planning [ 22 ] . Furthermore, AL has been utilized in SRRG2 for localization and control of unicycle robots [ 23 ] and for pseudo-omnidirectional platform control [ 24 ] . While these methods have been successfully applied to various robotics and control problems, existing approaches still suffer from key limitations as described below. I-B Motivation Incorporating equality constraints into factor graphs has been primarily achieved through two approaches: soft constraints and the Augmented Lagrangian (AL) method. In the soft constraints approach, enforcing equality constraints requires assigning an infinitely large weighting matrix, which is not feasible in practical implementations. Instead, a large but finite weighting matrix is typically used. However, choosing an appropriate weight requires careful tuning to strike a balance between effectively enforcing the constraint and avoiding ill-conditioned optimization problems. If the original optimization problem is already ill-conditioned, determining an appropriate weighting matrix becomes even more challenging. In contrast, the Augmented Lagrangian (AL) method systematically enforces equality constraints without requiring direct weight tuning. However, AL has notable limitations. First, as previously mentioned, it employs a nested loop structure, which may lead to unnecessary iterations in the inner loop. Second, the performance of the AL method is highly dependent on hyperparameter tuning, including the initial penalty term, penalty update factor, maximum penalty value, inner-loop iteration limit, and stopping criteria. Suboptimal parameter selection can significantly degrade performance. To address these limitations, we propose an alternative approach that extends the Gauss-Newton and Levenberg–Marquardt methods, which are the primary unconstrained optimization algorithms used in factor graphs, by incorporating Karush-Kuhn-Tucker (KKT) conditions to explicitly enforce equality constraints. This approach offers faster convergence and eliminates the need for extensive hyperparameter tuning. Furthermore, despite g2o’s efficiency in robotic perception tasks, it does not provide built-in mechanisms for explicitly handling equality constraints. This limitation necessitates either manual constraint encoding using soft constraints (which leads to tuning challenges) or extending the solver framework itself. I-C Contributions Our contributions are summarized as follows: 1. Algorithmic Contribution • We propose a KKT system-based Gauss-Newton method for efficiently handling equality constraints within factor graphs. • We formulate the implementation of equality constraints using the proposed method by representing them as factor graph