1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
|
# 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]):
...
```
`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:
```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]):
"""
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.
|
