env/gtsam-4.1.1/python/CustomFactors.md

GTSAM Python-based factors

One now can build factors purely in Python using the CustomFactor factor.

Usage

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

import gtsam
import numpy as np
from typing import List

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

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).

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 numpy array, as the Jacobian for the corresponding variable.

After defining error_func, one can create a CustomFactor just like any other factor in GTSAM:

noise_model = gtsam.noiseModel.Unit.Create(3)
# constructor(<noise model>, <list of keys>, <error callback>)
cf = gtsam.CustomFactor(noise_model, [X(0), X(1)], error_func)

Example

The following is a simple BetweenFactor implemented in Python.

import gtsam
import numpy as np
from typing import List

expected = Pose2(2, 2, np.pi / 2)

def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]):
    """
    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 = expected.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

noise_model = gtsam.noiseModel.Unit.Create(3)
cf = gtsam.CustomFactor(noise_model, gtsam.KeyVector([0, 1]), error_func)

In general, the Python-based factor works just like their C++ counterparts.

Known Issues

Because of the pybind11-based translation, the performance of CustomFactor is not guaranteed. Also, because pybind11 needs to lock the Python GIL lock for evaluation of each factor, parallel evaluation of CustomFactor is not possible.

Implementation

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 worth 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&, const JacobianVector*)>;

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.