NonlinearEqualityConstraint

Overview¶

NonlinearEqualityConstraint defines constraints of the form h(x)=0 and serves as the base for expression-based and zero-cost equality constraints.

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¶

ExpressionEqualityConstraint builds equality constraints from expressions and right-hand-side targets.
ZeroCostConstraint turns an existing noise-model factor into an equality constraint by forcing zero residual.
NonlinearEqualityConstraints provides a factor-graph container with utilities such as violation vectors and penalty-graph construction.

Mathematical Formulation¶

Equality constraints enforce:

h(x)=0.

(1)

In penalty form, this contributes:

\frac{\mu}{2}\|h(x)\|^2.

(2)

ExpressionEqualityConstraint<T> models $h(x)=r$ directly from expressions, while ZeroCostConstraint turns an existing noise-model factor residual into an equality constraint.

Key User API¶

ExpressionEqualityConstraint<T>(expression, rhs, sigmas)
ZeroCostConstraint(factor)
NonlinearEqualityConstraints::FromCostGraph(graph)
NonlinearEqualityConstraints::violationVector(values)
NonlinearEqualityConstraints::violationNorm(values)
NonlinearEqualityConstraints::penaltyGraph(mu)

Concise C++ Example¶

#include <gtsam/constrained/NonlinearEqualityConstraint.h>
#include <gtsam/nonlinear/expressions.h>

using namespace gtsam;

Symbol x1('x', 1), x2('x', 2);
Double_ X1(x1), X2(x2);

NonlinearEqualityConstraints eq;
eq.emplace_shared<ExpressionEqualityConstraint<double>>(
    X1 + X1 * X1 * X1 + X2 + X2 * X2, 0.0, Vector1(0.1));

Values values;
values.insert(x1, 1.0);
values.insert(x2, 1.0);

double norm = eq.violationNorm(values);
NonlinearFactorGraph penalty = eq.penaltyGraph(4.0);