12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Moved stuff to notebook, Jacobian guidance

release/4.3a0
Frank Dellaert 2025-04-06 14:08:45 -04:00
parent 3e8a29ae13
commit 02150a2f90
2 changed files with 133 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

144
gtsam/nonlinear/doc/CustomFactor.ipynb
View File

@ -18,7 +18,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 1,
"id": "5ccb48e4",
"metadata": {
"tags": [
@ -28,7 +28,17 @@
"languageId": "markdown"
}
},
"outputs": [],
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\u001b[31mERROR: Could not find a version that satisfies the requirement gtsam-develop (from versions: none)\u001b[0m\u001b[31m\n",
"\u001b[0m\u001b[31mERROR: No matching distribution found for gtsam-develop\u001b[0m\u001b[31m\n",
"\u001b[0mNote: you may need to restart the kernel to use updated packages.\n"
]
}
],
"source": [
"%pip install --quiet gtsam-develop"
]
@ -54,24 +64,58 @@
"\n",
"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.\n",
"\n",
"## Usage\n",
"## Usage in Python\n",
"\n",
"To use `CustomFactor`, users must:\n",
"In order to use a Python-based factor, one needs to have a Python function with the following signature:\n",
"\n",
"```python\n",
"def error_func(this: gtsam.CustomFactor, v: gtsam.Values, H: list[np.ndarray]) -> np.ndarray:\n",
" ...\n",
"```\n",
"\n",
"**Explanation**:\n",
"- `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). \n",
"- the error returned must be a 1D `numpy` array.\n",
"- If `H` is `None`, it means the current factor evaluation does not need Jacobians. For example, the `error`\n",
"method on a factor does not need Jacobians, so we don't evaluate them to save CPU. If `H` is not `None`,\n",
"each entry of `H` can be assigned a (2D) `numpy` array, as the Jacobian for the corresponding variable.\n",
"- All `numpy` matrices inside should be using `order=\"F\"` to maintain interoperability with C++.\n",
"\n",
"After defining `error_func`, one can create a `CustomFactor` just like any other factor in GTSAM. In summary, to use `CustomFactor`, users must:\n",
"1. Define the custom error function that models the specific measurement or constraint.\n",
"2. Implement the calculation of the Jacobian matrix for the error function.\n",
"3. Define a noise model of the appropriate dimension.\n",
"3. Add the `CustomFactor` to a factor graph, specifying\n",
" - the noise model\n",
" - the keys of the variables it depends on\n",
" - the error function\n",
" - the error function"
]
},
{
"cell_type": "markdown",
"id": "c7ec3512",
"metadata": {},
"source": [
"**Notes**:\n",
"- 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. \n",
"- Because `pybind11` needs to lock the Python GIL lock for evaluation of each factor, parallel evaluation of `CustomFactor` is not possible.\n",
"- You can mitigate both of these by having a python function that leverages batching of measurements.\n",
"\n",
"Some more examples of usage in python are given in [test_custom_factor.py](https://github.com/borglab/gtsam/blob/develop/python/gtsam/tests/test_custom_factor.py),[CustomFactorExample.py](https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/CustomFactorExample.py), and [CameraResectioning.py](https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/CameraResectioning.py)."
]
},
{
"cell_type": "markdown",
"id": "68a66627",
"metadata": {},
"source": [
"## Example\n",
"Below is a simple example that mimics a `BetweenFactor<Pose2>`."
]
},
{
"cell_type": "code",
"execution_count": 14,
"execution_count": 2,
"id": "894bfaf2",
"metadata": {},
"outputs": [
@ -89,9 +133,9 @@
"import numpy as np\n",
"from gtsam import CustomFactor, noiseModel, Values, Pose2\n",
"\n",
"measurement = Pose2(2, 2, np.pi / 2)\n",
"measurement = Pose2(2, 2, np.pi / 2) # is used to create the error function\n",
"\n",
"def error_func(this: CustomFactor, v: Values, H: list[np.ndarray]):\n",
"def error_func(this: CustomFactor, v: Values, H: list[np.ndarray]=None):\n",
" \"\"\"\n",
" Error function that mimics a BetweenFactor\n",
" :param this: reference to the current CustomFactor being evaluated\n",
@ -126,7 +170,7 @@
},
{
"cell_type": "code",
"execution_count": 15,
"execution_count": 3,
"id": "c92caf2c",
"metadata": {},
"outputs": [
@ -163,12 +207,88 @@
},
{
"cell_type": "markdown",
"id": "38c04012",
"id": "d9b61f83",
"metadata": {},
"source": [
"Note: 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. You can mitigate this by having a python function that leverages batching of measurements.\n",
"## Beware of Jacobians!\n",
"\n",
"Some more examples of usage in python are given in [test_custom_factor.py](https://github.com/borglab/gtsam/blob/develop/python/gtsam/tests/test_custom_factor.py) and [CustomFactorExample.py](https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/CustomFactorExample.py)."
"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\n",
"$$\n",
"\\lim_{\\xi\\rightarrow0}\\frac{\\left|f(a)+H_a\\xi-f\\left(a \\, \\text{Exp}(\\xi)\\right)\\right|}{\\left|\\xi\\right|}=0,\n",
"$$\n",
"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](https://github.com/borglab/gtsam/blob/develop/gtsam/doc/math.pdf).\n",
"\n",
"To test your Jacobians, you can use the handy `gtsam.utils.numerical_derivative` module. We give an example below:"
]
},
{
"cell_type": "code",
"execution_count": 4,
"id": "c815269f",
"metadata": {},
"outputs": [],
"source": [
"from gtsam.utils.numerical_derivative import numericalDerivative21, numericalDerivative22\n",
"\n",
"# Allocate the Jacobians and call error_func\n",
"H = [np.empty((6, 6), order='F'),np.empty((6, 6), order='F')]\n",
"error_func(custom_factor, values, H)\n",
"\n",
"# We use error_func directly, so we need to create a binary function constructing the values.\n",
"def f (T1, T2):\n",
" v = Values()\n",
" v.insert(66, T1)\n",
" v.insert(77, T2)\n",
" return error_func(custom_factor, v)\n",
"numerical0 = numericalDerivative21(f, values.atPose2(66), values.atPose2(77))\n",
"numerical1 = numericalDerivative22(f, values.atPose2(66), values.atPose2(77))\n",
"\n",
"# Check the numerical derivatives against the analytical ones\n",
"np.testing.assert_allclose(H[0], numerical0, rtol=1e-5, atol=1e-8)\n",
"np.testing.assert_allclose(H[1], numerical1, rtol=1e-5, atol=1e-8)"
]
},
{
"cell_type": "markdown",
"id": "fd09b0fc",
"metadata": {},
"source": [
"## Implementation Notes\n",
"\n",
"`CustomFactor` is a `NonlinearFactor` that has a `std::function` as its callback.\n",
"This callback can be translated to a Python function call, thanks to `pybind11`'s functional support.\n",
"\n",
"The constructor of `CustomFactor` is\n",
"```c++\n",
"/**\n",
"* Constructor\n",
"* @param noiseModel shared pointer to noise model\n",
"* @param keys keys of the variables\n",
"* @param errorFunction the error functional\n",
"*/\n",
"CustomFactor(const SharedNoiseModel& noiseModel, const KeyVector& keys, const CustomErrorFunction& errorFunction) :\n",
" Base(noiseModel, keys) {\n",
" this->error_function_ = errorFunction;\n",
"}\n",
"```\n",
"\n",
"At construction time, `pybind11` will pass the handle to the Python callback function as a `std::function` object.\n",
"\n",
"Something that deserves a special mention is this:\n",
"```c++\n",
"/*\n",
" * NOTE\n",
" * ==========\n",
" * pybind11 will invoke a copy if this is `JacobianVector &`,\n",
" * and modifications in Python will not be reflected.\n",
" *\n",
" * This is safe because this is passing a const pointer, \n",
" * and pybind11 will maintain the `std::vector` memory layout.\n",
" * Thus the pointer will never be invalidated.\n",
" */\n",
"using CustomErrorFunction = std::function<Vector(const CustomFactor&, const Values&, const JacobianVector*)>;\n",
"```\n",
"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.\n"
]
}
],

112
python/CustomFactors.md
View File

@ -1,114 +1,4 @@
# GTSAM Python-based factors
One now can build factors purely in Python using the `CustomFactor` factor.
One now can build factors purely in Python using the `CustomFactor` factor. See [this notebook](../gtsam/nonlinear/doc/CustomFactor.ipynb) for usage.
## 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.
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:
```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.