CustomFactor - GTSAM Docs

Notes:

There are not a lot of restrictions on the function, but note there is overhead in calling a python function from within a c++ optimization loop.
Because pybind11 needs to lock the Python GIL lock for evaluation of each factor, parallel evaluation of CustomFactor is not possible.
You can mitigate both of these by having a python function that leverages batching of measurements.

Some more examples of usage in python are given in test_custom_factor.py,CustomFactorExample.py, and CameraResectioning.py.

import gtsam
import numpy as np
from gtsam.symbol_shorthand import L, X

Overview¶

The CustomFactor class allows users to define custom error functions and Jacobians, and while it can be used in C++, it is particularly useful for use with the python wrapper.

Custom Error Function¶

The CustomFactor class allows users to define a custom error function. In C++ it is defined as below:

using JacobianVector = std::vector<Matrix>;
using CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, OptionalJacobianVector)>;

The function will be passed a reference to the factor itself so the keys can be accessed, a Values reference, and a writeable vector of Jacobians.

Usage in Python¶

In order to use a Python-based factor, one needs to have a Python function with the following signature:

def error_func(this: gtsam.CustomFactor, v: gtsam.Values, H: list[np.ndarray]) -> np.ndarray:
    ...

Explanation:

this is a reference to the CustomFactor object. This is required because one can reuse the same error_func for multiple factors. v is a reference to the current set of values, and H is a list of references to the list of required Jacobians (see the corresponding C++ documentation).
the error returned must be a 1D numpy array.
If H is None, it means the current factor evaluation does not need Jacobians. For example, the error method on a factor does not need Jacobians, so we don’t evaluate them to save CPU. If H is not None, each entry of H can be assigned a (2D) numpy array, as the Jacobian for the corresponding variable.
All numpy matrices inside should be using order="F" to maintain interoperability with C++.

After defining error_func, one can create a CustomFactor just like any other factor in GTSAM. In summary, to use CustomFactor, users must:

Define the custom error function that models the specific measurement or constraint.
Implement the calculation of the Jacobian matrix for the error function.
Define a noise model of the appropriate dimension.
Add the CustomFactor to a factor graph, specifying
- the noise model
- the keys of the variables it depends on
- the error function

Example¶

Below is a simple example that mimics a BetweenFactor<Pose2>.

import numpy as np
from gtsam import CustomFactor, noiseModel, Values, Pose2

measurement = Pose2(2, 2, np.pi / 2) # is used to create the error function

def error_func(this: CustomFactor, v: Values, H: list[np.ndarray]=None):
    """
    Error function that mimics a BetweenFactor
    :param this: reference to the current CustomFactor being evaluated
    :param v: Values object
    :param H: list of references to the Jacobian arrays
    :return: the non-linear error
    """
    key0 = this.keys()[0]
    key1 = this.keys()[1]
    gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
    error = measurement.localCoordinates(gT1.between(gT2))

    if H is not None:
        result = gT1.between(gT2)
        H[0] = -result.inverse().AdjointMap()
        H[1] = np.eye(3)
    return error

# we use an isotropic noise model, and keys 66 and 77
noise_model = noiseModel.Isotropic.Sigma(3, 0.1)
custom_factor = CustomFactor(noise_model, [66, 77], error_func)
print(custom_factor)

CustomFactor on 66, 77
isotropic dim=3 sigma=0.1

Typically, you would not actually call methods of a custom factor directly: a nonlinear optimizer will call linearize in every nonlinear iteration. But if you wanted to, here is how you would do it:

values = Values()
values.insert(66, Pose2(1, 2, np.pi / 2))
values.insert(77, Pose2(-1, 4, np.pi))

print("error = ", custom_factor.error(values))
print("Linearized JacobianFactor:\n", custom_factor.linearize(values))

error =  0.0
Linearized JacobianFactor:
   A[66] = [
	-6.12323e-16, -10, -20;
	10, -6.12323e-16, -20;
	-0, -0, -10
]
  A[77] = [
	10, 0, 0;
	0, 10, 0;
	0, 0, 10
]
  b = [ -0 -0 -0 ]
  No noise model

Beware of Jacobians!¶

It is important to unit-test the Jacobians you provide, because the convention used in GTSAM frequently leads to confusion. In particular, GTSAM updates variables using an exponential map on the right. In particular, for a variable $x\in G$ , an n-dimensional Lie group, the Jacobian $H_a$ at $x=a$ is defined as the linear map satisfying

\lim_{\xi\rightarrow0}\frac{\left|f(a)+H_a\xi-f\left(a \, \text{Exp}(\xi)\right)\right|}{\left|\xi\right|}=0,

(1)

where $\xi$ is a n-vector corresponding to an element in the Lie algebra $\mathfrak{g}$ , and $\text{Exp}(\xi)\doteq\exp(\xi^{\wedge})$ , with $\exp$ the exponential map from $\mathfrak{g}$ back to $G$ . The same holds for n-dimensional manifold $M$ , in which case we use a suitable retraction instead of the exponential map. More details and examples can be found in doc/math.pdf.

To test your Jacobians, you can use the handy gtsam.utils.numerical_derivative module. We give an example below:

from gtsam.utils.numerical_derivative import numericalDerivative21, numericalDerivative22

# Allocate the Jacobians and call error_func
H = [np.empty((6, 6), order='F'),np.empty((6, 6), order='F')]
error_func(custom_factor, values, H)

# We use error_func directly, so we need to create a binary function constructing the values.
def f (T1, T2):
    v = Values()
    v.insert(66, T1)
    v.insert(77, T2)
    return error_func(custom_factor, v)
numerical0 = numericalDerivative21(f, values.atPose2(66), values.atPose2(77))
numerical1 = numericalDerivative22(f, values.atPose2(66), values.atPose2(77))

# Check the numerical derivatives against the analytical ones
np.testing.assert_allclose(H[0], numerical0, rtol=1e-5, atol=1e-8)
np.testing.assert_allclose(H[1], numerical1, rtol=1e-5, atol=1e-8)

Implementation Notes¶

CustomFactor is a NonlinearFactor that has a std::function as its callback. This callback can be translated to a Python function call, thanks to pybind11’s functional support.

The constructor of CustomFactor is

/**
* Constructor
* @param noiseModel shared pointer to noise model
* @param keys keys of the variables
* @param errorFunction the error functional
*/
CustomFactor(const SharedNoiseModel& noiseModel, const KeyVector& keys, const CustomErrorFunction& errorFunction) :
  Base(noiseModel, keys) {
  this->error_function_ = errorFunction;
}

At construction time, pybind11 will pass the handle to the Python callback function as a std::function object.

Something that deserves a special mention is this:

/*
 * NOTE
 * ==========
 * pybind11 will invoke a copy if this is `JacobianVector &`,
 * and modifications in Python will not be reflected.
 *
 * This is safe because this is passing a const pointer, 
 * and pybind11 will maintain the `std::vector` memory layout.
 * Thus the pointer will never be invalidated.
 */
using CustomErrorFunction = std::function<Vector(const CustomFactor&, const Values&, OptionalJacobianVector)>;

which is not documented in pybind11 docs. One needs to be aware of this if they wanted to implement similar “mutable” arguments going across the Python-C++ boundary.