NonlinearFactor - GTSAM Docs

Overview¶

The NonlinearFactor class in GTSAM is a fundamental component used in nonlinear optimization. It represents a factor in a factor graph. The class is designed to work with nonlinear, continuous functions.

Mathematical Formulation¶

The NonlinearFactor is generally represented by a function $f(x)$ , where $x$ is a vector of variables. The error is given by:

e(x) = f(x)- z

(1)

where $z$ is the observed measurement. The optimization process aims to minimize the sum of squared errors:

\min_x \sum_i \| e_i(x) \|^2

(2)

Linearization involves approximating $f(x)$ around a point $x_0$ :

f(x) \approx f(x_0) + A\delta x

(3)

where $A$ is the Jacobian matrix of $f$ at $x_0$ , and $\delta x \doteq x - x_0$ . This leads to a linearized error:

e(x) \approx (f(x_0) + A\delta x) - z = A\delta x - b

(4)

where $b\doteq z - f(x_0)$ is the prediction error.

Key Functionalities¶

Error Calculation¶

evaluateError: This method computes the error vector for the factor given a set of variable values. The error is typically the difference between the predicted measurement and the actual measurement. The function can also return the Jacobian matrices if needed, which are crucial for optimization algorithms like Gauss-Newton or Levenberg-Marquardt.

Jacobian and Hessian¶

linearize: This method linearizes the nonlinear factor around a linearization point. It returns a GaussianFactor, which is an approximation of the NonlinearFactor using a first-order Taylor expansion. This is a critical step in iterative optimization methods, where the problem is repeatedly linearized and solved.

Active Flag¶

active: This function checks whether the factor should be included in the optimization process. A factor might be inactive if it does not contribute to the error, which can occur in cases of conditional constraints or gating functions.

Dimensionality¶

dim: Returns the dimensionality of the factor, which corresponds to the size of the error vector. This is important for understanding the contribution of the factor to the overall optimization problem.

Key Management¶

keys: Provides access to the keys (or variable indices) involved in the factor. This is essential for understanding which variables the factor is connected to in the factor graph.

Usage Notes¶

The NonlinearFactor class is typically used in conjunction with a NonlinearFactorGraph, which is a collection of such factors.
Users need to implement the evaluateError method in derived classes to define the specific measurement model.
The class is designed to be flexible and extensible, allowing for custom factors to be created for specific applications.