PenaltyOptimizer - GTSAM Docs

Overview¶

PenaltyOptimizer solves constrained problems by iteratively minimizing a merit function that adds weighted penalty terms for equality and inequality violations.

Authors: Frank Dellaert, et al. (see THANKS for the full author list)

See LICENSE for the license information

try:
    import google.colab
    %pip install --quiet gtsam-develop
except ImportError:
    pass

Key Concepts¶

PenaltyOptimizerParams controls initial penalty weights and update rates.
meritFunction builds the cost-plus-penalty nonlinear factor graph for each outer iteration.
Each outer iteration solves an unconstrained nonlinear problem, then increases penalties as needed.
progress stores per-iteration diagnostic information.

Mathematical Formulation¶

Penalty method solves a sequence of unconstrained problems:

m(x)=\frac{1}{2}\|f(x)\|^2 + \frac{\mu_{eq}}{2}\|h(x)\|^2 + \frac{\mu_{ineq}}{2}\|\text{ramp}(g(x))\|^2.

(1)

As outer iterations proceed, penalty weights increase to enforce feasibility more strongly.

Key User API¶

PenaltyOptimizer(problem, initialValues, params)
optimize()
progress()
PenaltyOptimizerParams: initialMuEq, initialMuIneq, increase rates, ineqConstraintPenaltyFunction, lmParams

Concise C++ Example¶

#include <gtsam/constrained/PenaltyOptimizer.h>

using namespace gtsam;

auto params = std::make_shared<PenaltyOptimizerParams>();
params->verbose = true;
params->initialMuEq = 1.0;
params->initialMuIneq = 1.0;

PenaltyOptimizer optimizer(problem, init_values, params);
Values results = optimizer.optimize();