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.

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.

Figures

The factor graph for the optimal control problem. The red circles represent the cost terms as regular factors, while the blue circles illustrate the representation of equality constraints as regular factors. k∈ {0, … , N-1}.

Optimal velocity trajectory obtained from the factor graph-based optimal control problem.

Iteration number across different optimization scenarios and trajectory lengths.

Computation time per iteration for different trajectory lengths using the Proposed Method and Augmented Lagrangian (AL) Method.

Inner iterations number per outer iteration index for different methods and initial guesses.

Tables

TABLE 1. Case study 2: iterations number to convergence for each method and initial guess.

Method	x(0)=(−1,−1)	x(0)=(−0.2,−0.2)	x(0)=(0,0)	x(0)=(2,2)	Avg.
Penalty (GTSAM)	56	75	79	82	73.0
AL (GTSAM)	72	57	61	98	72.0
AL (ecg2o)	20	20	19	21	20.0
KKT-based (ecg2o)	6	4	1	9	5.0

Full text

Introduction

1

Factor graphs are extensively used in robotic perception tasks for efficiently modeling large-scale probabilistic inference problems (Dellaert and Kaess, 2017). These methods use graph-based optimization techniques, such as weighted least squares, to address key problems like SLAM and situational awareness (Tourani et al., 2022; Bavle et al., 2022). Typically, these optimization problems are addressed using second-order unconstrained algorithms, including Gauss-Newton (Lai et al., 2017) and Levenberg–Marquardt (Moré, 2006). Several well-established libraries, such as GTSAM (Dellaert et al., 2022), g2o (Kümmerle et al., 2011), and SRRG2 (Grisetti et al., 2020), 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 also encompass optimal control applications. The primary challenge in this extension lies in handling constraints, where optimal control problems inherently involve hard constraints, unlike perception problems which are typically soft-constrained. However, factor graph optimization can provide a unified framework for perception and control tasks in robotics. Furthermore, this approach facilitates the reuse of established and computationally efficient algorithms available in factor graph libraries (Abdelkarim et al., 2025). In contrast, traditional optimization frameworks widely used in optimal control, such as CasADi (Andersson et al., 2019), AMPL (Abdelkarim and Zhang, 2020; Abdelkarim et al., 2023), and IPOPT (Wächter and Biegler, 2006), although highly effective, do not naturally integrate with factor graph–based optimization 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, hard equality constraints define strict and deterministic relationships between variables. A typical example is the system dynamics, which must be satisfied exactly to ensure that state transitions remain physically consistent. In contrast, the motion and sensor models in SLAM establish probabilistic or soft constraints between variables. These constraints do not enforce exact relationships but instead incorporate uncertainty directly into the factor graph formulation. As a result, hard equality constraints enforce exact dependencies to guarantee consistency, e.g., with the underlying physics, while soft probabilistic constraints model uncertainty and enable more flexible estimation.

In this work, we address the integration of hard equality constraints within factor graphs. Our contributions can be summarized as follows: (i) Method: we introduce an Sequential Quadratic Programming (SQP)-inspired approach that leverages the Karush-Kuhn-Tucker (KKT) conditions to natively integrate hard equality constraints into the factor graph framework. (ii) Implementation: we provide a lightweight, header-only C++ library, ecg2o, that extends the g2o optimization framework with support for both the Augmented Lagrangian (AL) method and the proposed KKT-based approach. The library is publicly available at https://github.com/snt-arg/ecg2o to foster reproducibility and further research. In addition, we introduce a stopping criterion based on the norm of the update step in g2o, ensuring termination once the step size becomes sufficiently small. (iii) Validation: we conduct a case study on trajectory-tracking optimal control, demonstrating the effectiveness of the proposed method. Furthermore, we benchmark our implementations against the penalty and AL methods in GTSAM, highlighting the efficiency and robustness of the implemented approaches.

The remainder of this paper is organized as follows. Section 2 reviews the state of the art in factor-graph-based optimization and motivates the need for more efficient methods for equality constraint handling. Section 3 introduces the necessary preliminaries, including weighted least squares, Gauss–Newton for unconstrained optimization, and the soft-constraint formulation. Section 4 presents the AL and the proposed KKT-based Gauss–Newton approaches. Section 5 reports the experimental evaluation comprising two case studies: trajectory-tracking optimal control and a nonlinear least-squares problem with equality constraints, where the proposed implementation is also compared against the penalty and AL methods in GTSAM. Finally, Section 6 concludes the paper and outlines directions for future work.

State of the art and motivation

2

State of the art

2.1

While factor graphs have initially been introduced to handle robotic perception tasks, they are increasingly used also for robotic optimal control problems, as summarized in the comprehensive review (Abdelkarim et al., 2025) along with a description of various methods for contsraint handling. In the following, we provide an overview of the key literature in this area.

Chen et al., 2019 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. (2021) 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 (Darnley, 2021), tactile estimation and extrinsic contact control (Kim et al., 2023), and trajectory generation for graffiti robots (Chen et al., 2022a; Chen et al., 2022b). 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 AL method (Bertsekas, 2014). 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 AL function. The optimization process involves two nested loops: the inner loop minimizes the AL 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 (Sodhi et al., 2020) and has been applied in navigation and 3D manipulation planning (Qadri et al., 2022). Furthermore, the AL has been utilized in SRRG2 for localization and control of unicycle robots (Bazzana et al., 2022) and for pseudo-omnidirectional platform control (Bazzana et al., 2024).

While these methods have been successfully applied to various robotics and control problems, existing approaches still suffer from key limitations as described below.

Motivation

2.2

Incorporating equality constraints into factor graphs has been primarily achieved through two approaches: soft constraints and the 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 achieve 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 Abdelkarim et al. (2025).

In contrast, the AL method systematically enforces equality constraints without requiring direct weight tuning. However, the 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 Nocedal and Wright (2006), 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 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.

Preliminaries

3

Weighted least squares in factor graphs

3.1

Factor graphs consist of variable nodes and factor nodes and represent a factorization of probability density functions (PDFs), which are typically assumed to follow a Gaussian distribution. In maximum a posteriori (MAP) inference, the objective is to maximize the factorized probability function, mathematically formulated in Equation 1.

$$[eqn]$$

Here, $[eqn]$ represents the exponential function, $[eqn]$ denotes the Mahalanobis norm, and $[eqn]$ is the transpose operator for vectors or matrices. The term $[eqn]$ is an error function associated with the factor $[eqn]$ , which depends on a subset of the variable nodes, and $[eqn]$ is the information matrix of factor $[eqn]$ , computed as the inverse of the covariance matrix. Furthermore, $[eqn]$ is the vector containing all variable nodes $[eqn]$ .

Since maximizing the MAP objective is equivalent to minimizing the negative log-likelihood, this optimization problem can be reformulated as a weighted least squares problem (Abdelkarim et al., 2025, § III.B)

$$[eqn]$$

In the following sections, we present solutions to the optimization problem defined in Equation 2, first without hard constraints and then with incorporated hard equality constraints.

Gauss-newton method for unconstrained least-squares

3.2

In factor graphs, the factor nodes typically define a nonlinear error function. Thus, optimization methods operate on a linearized version of this function. This is achieved through first-order Taylor expansion described in Equation 3.

$$[eqn]$$

where $[eqn]$ is a linearization point and $[eqn]$ is the Jacobian matrix of the error function of the factor $[eqn]$ evaluated at the linearization point $[eqn]$ .

Applying the stationarity condition (the gradient of the cost function vanishes at an optimal point), the Gauss-Newton update step at iteration $[eqn]$ is computed as

$$[eqn]$$

where the contribution of the factor $[eqn]$ at iteration $[eqn]$ in building the linear system is

$$[eqn]$$ $$[eqn]$$

and map $[eqn]$ , in Equation 4, is an operator that maps the local $[eqn]$ and $[eqn]$ to the global $[eqn]$ and $[eqn]$ . This mapping accounts for the fact that error functions generally depend only on subsets of the variable nodes, and map $[eqn]$ ensures proper zero-padding where necessary.

Constructing and solving this linear system, presented in Equation 5, is a key computational step in factor graph optimization. Once the linear system is solved, the variable nodes are updated in each iteration as described in Equation 6.

$$[eqn]$$

Although the linear system formulation remains the same across unconstrained optimization methods, different techniques introduce modifications. For example, in the Levenberg-Marquardt method, numerical stability is improved by adding a positive damping term to the diagonal elements of the matrix $[eqn]$ , mitigating issues related to ill-conditioned matrices.

Equality constraints as soft constraints

3.3

For vector-valued equality constraints $[eqn]$ , a common approach is to add a quadratic penalty $[eqn]$ with a large positive-definite (typically diagonal) weighting matrix $[eqn]$ to the factor-graph objective in (2). This yields the unconstrained problem as presented in Equation 7.

$$[eqn]$$

In a factor graph, this is implemented by adding one cost factor per constraint with error $[eqn]$ and information matrix $[eqn]$ . In the limit $[eqn]$ , the optimizer drives $[eqn]$ . In practice, however, very large weights can cause ill-conditioning and sensitivity to variable/constraint scaling, so careful (problem-dependent) weighting or normalization is required.

Methodology

4

We consider the optimization problem formulated in Equation 2, now extended with linearly independent equality constraints:

$$[eqn]$$ $$[eqn]$$

The associated Lagrangian (Abdelkarim, 2020, § 2.4.2) for the optimization problem in Equation 8 is given in Equation 9.

$$[eqn]$$

where the first term is the standard factor-graph objective (regular factors) and the second term introduces the Lagrange multipliers $[eqn]$ for the equality constraints.

This section provides a brief overview of the two methods implemented in ecg2o: the AL method and the KKT-based approach. Throughout, we use $[eqn]$ .

Augmented lagrangian method

4.1

Inspired by the approach presented in (Bazzana et al., 2024), we implemented the AL method in ecg2o to enforce equality constraints. Particularly, the AL method adds a penalty term $[eqn]$ for each equality constraint, to the Lagrangian function. This results in the following augmented Lagrangian function:

$$[eqn]$$

where the penalty term in Equation 10 is weighted by the matrix described in Equation 11.

$$[eqn]$$

and $[eqn]$ represents a diagonal matrix of dimension $[eqn]$ . The penalty parameter $[eqn]$ . Can either be uniform across all constraints or vary based on the algorithm’s settings. Here, we assume a uniform $[eqn]$ . For all constraints.

Notably, in the AL, the penalty terms do not need to be excessively large, as discussed in (Nocedal and Wright, 2006, Example 17.4). The algorithm iteratively minimizes the AL function with respect to $[eqn]$ , using the stationary condition, which yields an update step similar to Equation 4.

While the contribution of regular factors remains unchanged (as described in Equation 5), the contribution of equality factor $[eqn]$ at iteration $[eqn]$ can be described as shown in Equation 12.

$$[eqn]$$ $$[eqn]$$

The AL algorithm implemented in the ecg2o library iteratively solves the inner-loop unconstrained optimization while incorporating both cost and equality factor contributions when constructing the linear system. Upon satisfying the termination criteria or reaching the maximum number of inner-loop iterations, the algorithm exits the inner loop and updates the Lagrange multipliers using Equation 13.

$$[eqn]$$

Optionally, we increase the penalties according to Equation 14.

$$[eqn]$$

where $[eqn]$ is the upper bound on the penalty parameter, and $[eqn]$ is the penalty update factor.

KKT-based gauss-newton method

4.2

In this section, we present an SQP-inspired method that enables factor graphs to natively solve equality-constrained optimization problems by leveraging KKT conditions. Unlike traditional approaches that require specialized algorithms like Penalty or Augmented Lagrangian methods, our approach demonstrates that factor graphs inherently possess the mathematical structure to handle equality constraints through appropriate factor design, eliminating the need for algorithmic modifications or nested loops.

Our core contribution is the formulation of equality constraints as regular factor nodes that, when combined with standard factor graph optimization, naturally enforce the KKT conditions of the original constrained problem. This transforms the equality constrained problem into an unconstrained optimization over an extended variable space that includes both original variables and Lagrange multipliers. Crucially, this formulation maintains the computational efficiency and sparsity exploitation of standard factor graph optimization while ensuring exact constraint satisfaction—unlike penalty methods that only achieve approximate satisfaction.

The mathematical foundation establishes that standard Gauss-Newton updates applied to our specially designed equality constraint factors yield iterations equivalent to SQP methods. This equivalence represents a significant theoretical advancement, demonstrating that factor graphs can directly implement sophisticated equality-constrained optimization without external algorithmic frameworks.

Consistent with the SQP framework, each subproblem is constructed by linearizing the nonlinear system. We begin by linearizing both the error functions $[eqn]$ and the equality constraints $[eqn]$ around the current estimate $[eqn]$ . Their linearized versions, denoted by $[eqn]$ and $[eqn]$ respectively, are given by:

$$[eqn]$$ $$[eqn]$$

The Lagrangian function for the linearized system in Equation 15 is given in Equation 16.

$$[eqn]$$

Let $[eqn]$ be the local optimal points. These must satisfy the KKT conditions (Boyd and Vandenberghe, 2004, §5.3.3) of the linearized optimization problem:

$$[eqn]$$

where $[eqn]$ , in Equation 17, denotes the gradient, and $[eqn]$ is the vector of all equality constraints.

Assuming $[eqn]$ , the KKT system is formulated as:

$$[eqn]$$

The introduction of equality constraints modifies the linear system compared to unconstrained optimization, as highlighted in red. The key challenge lies in representing these constraints within the factor graph using appropriate variables and factors. Specifically, how can we define the error function and the weighting matrix associated with the equality constraints to enforce them effectively?

Design of equality constraint factors

4.2.1

We propose defining a regular factor for each equality constraint such that it produces the same update step as in (18). Specifically, for an equality constraint $[eqn]$ , we define a regular factors with the following error function and weighting matrix:

$$[eqn]$$

where $[eqn]$ and $[eqn]$ have the dimension $[eqn]$ , and $[eqn]$ denotes the identity matrix. This formulation results in a linear system equivalent to the KKT system of the equality-constrained optimization problem. Consequently, it allows us to enforce equality constraints while maintaining an unconstrained factor graph structure. Furthermore, in this approach, the Lagrange multipliers are treated as variable nodes, naturally integrating them into the optimization process.

For ease of implementation, the ecg2o library provides a dedicated class for equality factors, which automatically incorporates Lagrange multipliers and defines the associated weighting matrix described in Equation 19. Additionally, the class simplifies the Jacobian implementation for equality factors, making the equality implementation as straightforward as for regular factors.

This design ensures an efficient and flexible extension of factor graphs, maintaining the computational advantages of existing optimization techniques while enabling constrained optimization in a natural manner.

Results and evaluation

5

In this section, we present two case studies: (i) an optimal control problem, and (ii) a comparison between the AL and penalty methods in GTSAM, and the AL and KKT-based methods in ecg2o.

Case study 1

5.1

We solve here an optimization problem that generates a sequence of control force inputs for an autonomous car to track a reference velocity trajectory. Our objective is to compare the performance of the AL method with our proposed approach, which models equality constraints as regular edges by incorporating Lagrange multipliers.

Problem formulation

5.1.1

Inspired by Jia et al. (2023), we formulate the optimal control problem as

$$[eqn]$$ $$[eqn]$$

where $[eqn]$ represents the vehicle velocity, $[eqn]$ is the input force applied to the car (measured in Newtons), and $[eqn]$ is the reference velocity. In addition, the subscript $[eqn]$ denotes the time instance, while $[eqn]$ represents the sampling time of the controller, which defines the horizon length.

The resistance force $[eqn]$ depends on the gravitational force $[eqn]$ , the rolling resistance $[eqn]$ , and the aerodynamic resistance force $[eqn]$ . The total resistance force is given by

$$[eqn]$$

The model is nonlinear due to the aerodynamic resistance force. The model parameters are as follows: $[eqn]$ represents the effective mass of the mass under the effect of the rotational mass of the powertrain, while $[eqn]$ denotes the actual mass of the vehicle. The gravitational constant is given by $[eqn]$ , and $[eqn]$ represents the road slope. The air density is denoted by $[eqn]$ , and $[eqn]$ is the frontal area of the vehicle. The coefficients $[eqn]$ and $[eqn]$ correspond to the air drag and rolling resistance coefficient, respectively.

The factor graph representing the optimal control problem in Equation 20 is illustrated in Figure 1, where the equality constraints are incorporated using our proposed approach.

The factor graph for the optimal control problem. The red circles represent the cost terms as regular factors, while the blue circles illustrate the representation of equality constraints as regular factors. k∈ {0, … , N-1}.

Evaluation

5.1.2

The reference trajectory for the velocity profile consists of 385 points with a sampling rate of 1 s. Using the factor graph-based optimization method, we solve the optimal control problem with weighting parameters $[eqn]$ and $[eqn]$ to determine the optimal input sequence. Applying this control sequence results in the optimal velocity profile. The velocity profile corresponding to the optimal control sequence obtained from solving the optimization problem is illustrated in Figure 2. While different controller performances can be achieved with varying parameter settings, our primary focus is to evaluate the optimizer itself. The solution of the optimization problem using the AL method results in a velocity trajectory that is quite similar to our proposed approach. This is because both methods share the same stopping criteria. We computed the root mean squared error (RMSE) between both trajectories to quantify their similarity. The RMSE between the velocity trajectories obtained from the AL method and our proposed approach is 0.2426, indicating a high degree of similarity.

Optimal velocity trajectory obtained from the factor graph-based optimal control problem.

Iteration numbers

5.1.2.1

In our case, the presence of nonlinear terms in the equality constraints might influence the number of iterations required for convergence. To address this effect, we considered an approximation for the nonlinear aerodynamic resistance term using a linear model:

$$[eqn]$$

where $[eqn]$ and $[eqn]$ , in Equation 22, are constants. Therefore, we considered five distinct scenarios for comparison. First, we solve the unconstrained optimization problem, where the constraints are ignored, serving as a baseline to understand the effect of enforcing constraints. Next, we apply our proposed method to both a linearized dynamic model and the nonlinear system, allowing us to analyze how constraint approximation influences performance. Additionally, we compare these results with the AL approach, solving the problem in both linearized and nonlinear forms. In addition, we consider three different reference trajectory lengths—5, 100, and 385—to evaluate the optimization performance across varying problem sizes. The results of the iteration number across all scenarios for the three trajectory lengths are summarized in Figure 3.

Iteration number across different optimization scenarios and trajectory lengths.

Figure 3 shows that the iteration number in the unconstrained optimization matches that of our proposed method with linearized dynamics, converging in just two iterations across all trajectory lengths. For nonlinear dynamics, our method requires slightly more iterations (3–4), reflecting the added complexity while maintaining efficiency.

In contrast, the AL method requires 11–13 iterations, approximately three times more than our proposed method, highlighting its higher computational cost. These results were obtained after careful parameter tuning, where we set the maximum number of inner iterations to 1, with $[eqn]$ , $[eqn]$ , and $[eqn]$ . However, further tuning is still required for different optimization problems.

Regarding sensitivity to the initial value of the Lagrange multiplier, we found that both algorithms maintained a stable iteration number across different Lagrange multiplier initial values.

Computation time

5.1.2.2

We compare the computation time for the nonlinear dynamics scenario across three different trajectory lengths. The optimization problem is solved 1,000 times, and the reported computation time represents the average over these runs to ensure consistency. All experiments were conducted on a Linux system with an Intel Core i9 12th Gen processor and 32 GB of RAM. The results are shown in Figure 4.

Computation time per iteration for different trajectory lengths using the Proposed Method and Augmented Lagrangian (AL) Method.

The computation time for the AL method is slightly lower than that of our proposed method, which was expected since our solver handles a larger linear system in each iteration. However, advancements in linear solvers have reduced the impact of solving larger systems on overall computation time, making the difference less apparent. The increase in computation time for our method ranges between $[eqn]$ and $[eqn]$ , but the significant reduction in the number of iterations makes it a worthwhile trade-off, especially for applications requiring fast convergence.

Case study 2: nonlinear least–squares with an equality constraint

5.2

For further validation of the implemented solvers, we adopted a nonlinear equality-constrained problem from the GTSAM library. In this case study, four solvers are considered: Penalty (GTSAM), AL (GTSAM), AL (ecg2o), and the KKT-based method (ecg2o). The constrained optimization problem is formulated in Equation 23.

$$[eqn]$$ $$[eqn]$$

A key distinction between our AL implementation in ecg2o and GTSAM’s approach lies in the inner iteration control strategy. In the AL method, each outer iteration solves an equivalent unconstrained optimization problem with fixed penalty parameters and Lagrange multipliers. While one could solve this inner problem to high accuracy, this is computationally inefficient since the penalty and multiplier estimates are updated in subsequent outer iterations. Therefore, we explicitly limit the maximum number of inner iterations to balance computational efficiency with sufficient progress toward constraint satisfaction. In ecg2o, we set this limit to five inner iterations per outer loop, whereas GTSAM employs a different convergence strategy without such explicit limits.

This difference is illustrated in Figure 5, which shows the number of inner iterations per outer iteration for each method. The choice of inner iteration limit represents a trade-off: too few iterations may hinder progress, while too many may waste computational effort on prematurely accurate inner solutions. Notably, when we increased the maximum inner iteration limit from 5 to 50 in our AL implementation for the initial guess $[eqn]$ , the total iteration count increased from 20 to 45 without additional improvement in solution quality.

Inner iterations number per outer iteration index for different methods and initial guesses.

The iteration numbers reported in Table 1 are measured consistently across implementations. In ecg2o, we directly count the number of inner iterations (i.e., iterations where the main calculations and linear system updates occur), while for GTSAM we sum the reported uopt_iters across all outer iterations to obtain a comparable metric. All solvers successfully converged to the optimal solution $[eqn]$ with accuracy better than $[eqn]$ across all initial guesses

$$[eqn]$$

The KKT-based Gauss-Newton method in ecg2o achieved the fastest convergence (average of five iterations), followed by AL in ecg2o (average of 20 iterations). Both GTSAM baselines required substantially more iterations (averages of 73 for penalty and 72 for AL). The performance advantage of our KKT-based approach is consistent across initial conditions, demonstrating its efficiency for this class of constrained problems.

We further investigated the effect of the penalty parameter $[eqn]$ on AL performance. Increasing $[eqn]$ from 1.5 to 2 reduced the iteration count from 20 to 18 for the $[eqn]$ case, as accelerated penalty growth drives faster constraint satisfaction. However, overly aggressive penalty increases risk numerical ill-conditioning, particularly in large-scale systems. This parameter sensitivity highlights a key advantage of our KKT-based method, which requires no such tuning while maintaining robust performance. The AL parameters used in this study were $[eqn]$ , $[eqn]$ , and $[eqn]$ , with a maximum of five inner iterations per outer loop.

Conclusion and future work

6

In this paper, we addressed the challenge of incorporating equality constraints into factor graphs, an essential extension for applications beyond robotic perception, such as optimal control. While the AL method is the state-of-the-art approach for handling equality constraints, we proposed an SQP-inspired, KKT-based method that natively integrates constraints within factor graph optimization without requiring additional iterative adjustments.

The proposed approach outperforms the AL method by achieving faster convergence and eliminating the need for parameter tuning, thereby enhancing its practicality. Its efficiency and robustness were validated through a case study on velocity tracking in autonomous vehicles. In addition, the implementation was compared against the Penalty method and AL in GTSAM, demonstrating superior performance.

As future work, we aim to explore more advanced constrained optimization techniques beyond AL, specifically targeting the handling of inequality constraints within factor graph frameworks. This would further expand the applicability of factor graphs to control and planning problems in robotics.