gtsam/gtsam/nonlinear/doc/NonlinearEquality.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# NonlinearEquality\n",
    "\n",
    "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."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## NonlinearEquality\n",
    "\n",
    "The `NonlinearEquality` factor enforces equality between a variable and a feasible value. It supports both exact and inexact evaluation modes.\n",
    "\n",
    "### Constructors\n",
    "- `NonlinearEquality(Key j, const T& feasible, const CompareFunction& compare)`  \n",
    "  Creates a factor that enforces exact equality between the variable at key `j` and the feasible value `feasible`.  \n",
    "  - `j`: Key of the variable to constrain.  \n",
    "  - `feasible`: The feasible value to enforce equality with.  \n",
    "  - `compare`: Optional comparison function (default uses `traits<T>::Equals`).\n",
    "\n",
    "- `NonlinearEquality(Key j, const T& feasible, double error_gain, const CompareFunction& compare)`  \n",
    "  Creates a factor that allows inexact evaluation with a specified error gain.  \n",
    "  - `error_gain`: Gain applied to the error when the constraint is violated.\n",
    "\n",
    "### Methods\n",
    "- `double error(const Values& c) const`  \n",
    "  Computes the error for the given values. Returns `0.0` if the constraint is satisfied, or a scaled error if `allow_error_` is enabled.\n",
    "\n",
    "- `Vector evaluateError(const T& xj, OptionalMatrixType H = nullptr) const`  \n",
    "  Evaluates the error vector for the given variable value `xj`. Optionally computes the Jacobian matrix `H`.\n",
    "\n",
    "- `GaussianFactor::shared_ptr linearize(const Values& x) const`  \n",
    "  Linearizes the factor at the given values `x` to create a Gaussian factor."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## NonlinearEquality1\n",
    "\n",
    "The `NonlinearEquality1` factor is a unary equality constraint that fixes a variable to a specific value.\n",
    "\n",
    "### Constructors\n",
    "- `NonlinearEquality1(const X& value, Key key, double mu = 1000.0)`  \n",
    "  Creates a factor that fixes the variable at `key` to the value `value`.  \n",
    "  - `value`: The fixed value for the variable.  \n",
    "  - `key`: Key of the variable to constrain.  \n",
    "  - `mu`: Strength of the constraint (default: `1000.0`).\n",
    "\n",
    "### Methods\n",
    "- `Vector evaluateError(const X& x1, OptionalMatrixType H = nullptr) const`  \n",
    "  Evaluates the error vector for the given variable value `x1`. Optionally computes the Jacobian matrix `H`.\n",
    "\n",
    "- `void print(const std::string& s, const KeyFormatter& keyFormatter) const`  \n",
    "  Prints the factor details, including the fixed value and noise model."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## NonlinearEquality2\n",
    "\n",
    "The `NonlinearEquality2` factor is a binary equality constraint that enforces equality between two variables.\n",
    "\n",
    "### Constructors\n",
    "- `NonlinearEquality2(Key key1, Key key2, double mu = 1e4)`  \n",
    "  Creates a factor that enforces equality between the variables at `key1` and `key2`.  \n",
    "  - `key1`: Key of the first variable.  \n",
    "  - `key2`: Key of the second variable.  \n",
    "  - `mu`: Strength of the constraint (default: `1e4`).\n",
    "\n",
    "### Methods\n",
    "- `Vector evaluateError(const T& x1, const T& x2, OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const`  \n",
    "  Evaluates the error vector for the given variable values `x1` and `x2`. Optionally computes the Jacobian matrices `H1` and `H2`.\n",
    "\n",
    "- `void print(const std::string& s, const KeyFormatter& keyFormatter) const`  \n",
    "  Prints the factor details, including the keys and noise model."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Common Features\n",
    "\n",
    "### Error Handling Modes\n",
    "- Exact Evaluation: Throws an error during linearization if the constraint is violated.  \n",
    "- Inexact Evaluation: Allows nonzero error and scales it using the `error_gain_` parameter.\n",
    "\n",
    "### Serialization\n",
    "All factors support serialization for saving and loading models.\n",
    "\n",
    "### Testable Interface\n",
    "All factors implement the `Testable` interface, providing methods like:\n",
    "- `void print(const std::string& s, const KeyFormatter& keyFormatter) const`  \n",
    "  Prints the factor details.\n",
    "- `bool equals(const NonlinearFactor& f, double tol) const`  \n",
    "  Checks if two factors are equal within a specified tolerance."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "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."
   ]
  }
 ],
 "metadata": {
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}