NonlinearEquality

The NonlinearEquality family of factors in GTSAM provides constraints to enforce equality between variables or between a variable and a constant value. These factors are useful in optimization problems where strict equality constraints are required. Below is an overview of the API, grouped by functionality.

NonlinearEquality¶

The NonlinearEquality factor enforces equality between a variable and a feasible value. It supports both exact and inexact evaluation modes.

Constructors¶

• NonlinearEquality(Key j, const T& feasible, const CompareFunction& compare)
  Creates a factor that enforces exact equality between the variable at key j and the feasible value feasible.

  • j: Key of the variable to constrain.
  • feasible: The feasible value to enforce equality with.
  • compare: Optional comparison function (default uses traits<T>::Equals).

• NonlinearEquality(Key j, const T& feasible, double error_gain, const CompareFunction& compare)
  Creates a factor that allows inexact evaluation with a specified error gain.

  • error_gain: Gain applied to the error when the constraint is violated.

Methods¶

• double error(const Values& c) const
  Computes the error for the given values. Returns 0.0 if the constraint is satisfied, or a scaled error if allow_error_ is enabled.

• Vector evaluateError(const T& xj, OptionalMatrixType H = nullptr) const
  Evaluates the error vector for the given variable value xj. Optionally computes the Jacobian matrix H.

• GaussianFactor::shared_ptr linearize(const Values& x) const
  Linearizes the factor at the given values x to create a Gaussian factor.

NonlinearEquality1¶

The NonlinearEquality1 factor is a unary equality constraint that fixes a variable to a specific value.

Constructors¶

• NonlinearEquality1(const X& value, Key key, double mu = 1000.0)
  Creates a factor that fixes the variable at key to the value value.
  • value: The fixed value for the variable.
  • key: Key of the variable to constrain.
  • mu: Strength of the constraint (default: 1000.0).

Methods¶

• Vector evaluateError(const X& x1, OptionalMatrixType H = nullptr) const
  Evaluates the error vector for the given variable value x1. Optionally computes the Jacobian matrix H.

• void print(const std::string& s, const KeyFormatter& keyFormatter) const
  Prints the factor details, including the fixed value and noise model.

NonlinearEquality2¶

The NonlinearEquality2 factor is a binary equality constraint that enforces equality between two variables.

Constructors¶

• NonlinearEquality2(Key key1, Key key2, double mu = 1e4)
  Creates a factor that enforces equality between the variables at key1 and key2.
  • key1: Key of the first variable.
  • key2: Key of the second variable.
  • mu: Strength of the constraint (default: 1e4).

Methods¶

• Vector evaluateError(const T& x1, const T& x2, OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const
  Evaluates the error vector for the given variable values x1 and x2. Optionally computes the Jacobian matrices H1 and H2.

• void print(const std::string& s, const KeyFormatter& keyFormatter) const
  Prints the factor details, including the keys and noise model.

Common Features¶

Error Handling Modes¶

• Exact Evaluation: Throws an error during linearization if the constraint is violated.
• Inexact Evaluation: Allows nonzero error and scales it using the error_gain_ parameter.

Serialization¶

All factors support serialization for saving and loading models.

Testable Interface¶

All factors implement the Testable interface, providing methods like:

• void print(const std::string& s, const KeyFormatter& keyFormatter) const
  Prints the factor details.
• bool equals(const NonlinearFactor& f, double tol) const
  Checks if two factors are equal within a specified tolerance.

These factors provide a flexible way to enforce equality constraints in nonlinear optimization problems, making them useful for applications like SLAM, robotics, and control systems.