113 lines
3.9 KiB
Markdown
113 lines
3.9 KiB
Markdown
# 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:
|
|
|
|
```python
|
|
import gtsam
|
|
import numpy as np
|
|
from typing import List
|
|
|
|
def error_func(this: gtsam.CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> 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). Note that
|
|
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.
|
|
|
|
After defining `error_func`, one can create a `CustomFactor` just like any other factor in GTSAM:
|
|
|
|
```python
|
|
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.
|
|
|
|
```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]) -> 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
|
|
```c++
|
|
/**
|
|
* 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:
|
|
```c++
|
|
/*
|
|
* 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.
|