# ecg2o: a seamless extension of g2o for equality-constrained factor graph optimization

## TL;DR

This paper introduces ecg2o, a new library that extends g2o to handle hard equality constraints in factor graph optimization, improving accuracy in robotic perception tasks.

## Contribution

A novel extension of factor graphs that natively supports hard equality constraints without additional optimization techniques.

## Key findings

- The proposed method achieves constraint satisfaction while maintaining the efficiency of second-order optimization.
- ecg2o is benchmarked against existing methods in g2o and GTSAM, showing competitive performance.
- An open-source C++ library is introduced to support equality-constrained optimization in robotic perception.

## 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 modeling. 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 hard equality constraints can yield more accurate state estimates and broaden their applicability, particularly in planning and control. Prior work has addressed equality handling either by soft penalties (large weights) or by nested-loop Augmented Lagrangian (AL) schemes. In this paper, we propose a novel extension of factor graphs that seamlessly incorporates hard equality constraints without requiring additional optimization techniques. Our approach maintains the efficiency and flexibility of existing second-order optimization techniques while ensuring constraint satisfaction. To validate the proposed method, an autonomous-vehicle velocity-tracking optimal control problem is solved and benchmarked against an AL baseline, both implemented in g2o. Additional comparisons are conducted in GTSAM, where the penalty method and AL are evaluated against our g2o implementations. Moreover, we introduce ecg2o, a header-only C++ library that extends the widely used g2o library with full support for hard 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.