The constrained module in GTSAM provides constrained nonlinear optimization on top of factor graphs.
It includes classes for representing constraints, building constrained problems, and solving them with penalty and augmented Lagrangian methods.
Core Problem Model¶
ConstrainedOptProblem: Holds objective costs, equality constraints, and inequality constraints.ConstrainedOptProblem::AuxiliaryKeyGenerator: Generates keys for auxiliary variables used when transforming inequality constraints.NonlinearConstraint: Base class for nonlinear constraints represented as constrainedNoiseModelFactorobjects.QpProblem: Quadratic programs with affine quadratic costs and linear constraints over directVectorandMatrixvalues, including guidance on sparse versus dense active-set subproblems.QP solver benchmarks: MPC and captured G1 comparisons against Clarabel, OSQP, and ProxQP, with a downloadable HTML report, graphics, and numerical checks.
LpProblem: Linear programs with linear costs and linear constraints over directVectorandMatrixvalues.
Equality Constraints¶
NonlinearEqualityConstraint: Base class for constraints of the formh(x) = 0.NonlinearEquality: Pins a variable to a constant value or enforces equality between two variables.ExpressionEqualityConstraint<T>: Equality constraint from an expression and right-hand side.ZeroCostConstraint: Equality constraint that enforces zero residual on a cost factor.NonlinearEqualityConstraints: Container graph for equality constraints.
Inequality Constraints¶
NonlinearInequalityConstraint: Base class for constraints of the formg(x) <= 0.BoundingConstraint: Base classes for unary and binary scalar bound constraints.ScalarExpressionInequalityConstraint: Scalar expression-based inequality constraint.NonlinearInequalityConstraints: Container graph for inequality constraints.InequalityPenaltyFunction: Interface for ramp-like penalty mappings used with inequality constraints. Derived classes:
QP and QCQP Problems¶
QpProblem: Holds affine quadratic costs and linear equality/inequality constraints over vector or matrix variables.QpCost: Affine quadratic objective term backed by a Hessian factor.LinearConstraint: Linear constraint represented as equal, less-equal, or greater-equal.ActiveSetSolver: Active-set QP/LP solver with sparse and dense QP subproblem modes.QcqpProblem: Holds quadratic costs and linear/quadratic constraints over vector or matrix variables.QpCost: Also used for QCQP objectives;QpCost(keys, Q, columnDim)creates a pure row-space quadratic cost over vectors or matrices .QuadraticConstraint: Scalar quadratic constraint , where is equal, less-equal, or greater-equal. For vector variables it may carry a linear term, , following Luo et al. (2010);QuadraticConstraint::FromPqraccepts the Boyd and Vandenberghe form (Convex Optimization, Eq. (4.35)).QcqpProblem(graph, columnDim, useRedundantConstraints): Opt-in conversion hook for supported nonlinear factors that can populateQpCostobjectives andQuadraticConstraintequalities over matrix-valued QCQP variables. WithuseRedundantConstraints, it also adds redundant constraints implied by the manifold constraints, at D=1 the column-orthonormality and cyclic cross-product identities of SO(3) for Rot3 and Pose3: they tighten SDP relaxations, but the QCQP then violates LICQ.insertQcqpValue<T, D>inserts supported QCQP variable values.QcqpVariableConstraints::addlazily generates each variable’s equality constraints once per key and collects them in insertion order. Ordinary and redundant variable constraints use separate collectors.extractQcqpValues<T, D>: Projection of exact-shape D=1 vectors or matrix slices back to manifold values. Absolute results from unanchored matrix components are gauge-dependent. Extraction is shape-only, so compact D=1 Rot2 and Vector2 blocks (both 2-by-1) require key-directed recovery when they appear in the sameValuescontainer.
D=1 QCQP vectors carry no homogenizing coordinate. Affine terms are stated
directly: as linear and constant terms of a QpCost, as the linear term of a
QuadraticConstraint, or as the right-hand side of a LinearConstraint. The
lifted SDP adds the constant 1 once per PSD block instead.
The leading factor of 1/2 in row-space QpCost construction is intentional:
it follows GTSAM’s standard factor-error convention. To represent a QCQP
objective written without the 1/2, pass twice the row-space Q blocks to
QpCost.
The rotation conversion has two tracks. Rot2 at D=1 uses the exact minimal
lift and supports a sign-pinning hard prior.
The identities and are structural, so its SDP
block has order 3 rather than 5, counting the constant entry. At D>=N, Rot2 (D>=2) and Rot3
(D>=3) use row-Stiefel variables satisfying . Between costs have a
common right- gauge. Matrix-form priors are intentionally unsupported: a
fixed target breaks that gauge and cannot be represented
by the Burer--Monteiro Gram matrix alone. A future BM-compatible lowering can
introduce an anchor block and use the invariant cost
. The Stiefel constraints do not enforce
determinant +1, so square variables also admit reflections. Unsupported
factors throw from NonlinearFactor::qcqpFactors.
Optimizers¶
ConstrainedOptimizerParams,ConstrainedOptimizerState,ConstrainedOptimizer: Shared base interfaces and iteration state for constrained solvers.PenaltyOptimizerParams,PenaltyOptimizerState,PenaltyOptimizer: Penalty method solver and its parameters/state.AugmentedLagrangianSolver: Direct Gaussian QP solver sharing the nonlinear optimizer’s ALM engine.AugmentedLagrangianParams,AugmentedLagrangianState,AugmentedLagrangianOptimizer: Augmented Lagrangian solver and its parameters/state.ActiveSetSolver: Active-set solver forQpProblemandLpProblem, with sparse and dense QP subproblem modes.
How the Pieces Fit Together¶
For a new user, it helps to think in two phases:
Build a constrained problem.
Run a constrained solver on that problem.
Inequality constraints can use different smooth penalty shapes via
InequalityPenaltyFunction (ramp, smooth polynomial ramps, or softplus),
which controls behavior near the active constraint boundary in
PenaltyOptimizer. AugmentedLagrangianOptimizer instead requires exact PHR
inequality terms and rejects custom smooth penalties so its projected
multiplier update remains mathematically consistent.
Both ALM frontends share one internal engine for penalty and multiplier updates,
LM damping, diagnostics and stopping. AugmentedLagrangianSolver accepts native
Gaussian graphs and VectorValues; AugmentedLagrangianOptimizer supplies
nonlinear function evaluations, Jacobians and manifold retraction. Each call
retains one Gaussian step graph and damped multifrontal workspace, with an
ordering and symbolic structure reused across outer iterations. A standalone
nonlinear iterate() call creates its own workspace.
The Gaussian adapter copies objective coefficient storage once, shifts only RHS/linear terms, and updates augmented constraint coefficients when penalties or PHR activity change. The nonlinear adapter reuses each original linearization for its step and stationarity tests. Inactive PHR rows retain zero Jacobians and their factor slots. Original whitened residuals supply outer diagnostics and projected multipliers directly. On terminal BCL convergence, those multipliers agree with the reported stationarity.
lmParams supplies damping, ordering, multifrontal settings and iteration
limits. Numerical elimination is repeated for attempted inner solves; numerical
factor reuse is not implemented. Stable model reduction uses gradient/Hessian
products; a trial-gradient check handles roundoff-sized merit differences.
Native ALM convergence does not include an absolute QP gap test. See the
Gaussian solver guide for a small driver,
multiplier units, counters and profiling.
The BCL inner stationarity schedule is bounded below by
absoluteStationarityTolerance. This lets feasibility updates continue
without demanding inner accuracy beyond the requested final tolerance.
timeAugmentedLagrangianPolicies --warmup 3 --repeats 21 benchmarks the policy
examples and sparse inequality chains with 512 and 2,048 variables, reporting
time, iteration counts, success, and final KKT residuals.
1) Build the Problem¶
This stage is about modeling: you separate what you want to minimize
(objective terms) from what must hold (constraints), then combine them into a
single ConstrainedOptProblem object that the solvers can consume.
2) Solve the Problem¶
This stage is algorithmic: pick a constrained solver, form iterative unconstrained subproblems internally, and solve those subproblems with a standard nonlinear optimizer until constraint violation and cost are reduced.