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", "cell_type": "code",
"execution_count": null, "execution_count": 1,
"id": "5ccb48e4", "id": "5ccb48e4",
"metadata": { "metadata": {
"tags": [ "tags": [
@ -28,7 +28,17 @@
"languageId": "markdown" "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": [ "source": [
"%pip install --quiet gtsam-develop" "%pip install --quiet gtsam-develop"
] ]
@ -54,24 +64,58 @@
"\n", "\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", "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", "\n",
"## Usage\n", "## Usage in Python\n",
"\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", "\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", "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", "2. Implement the calculation of the Jacobian matrix for the error function.\n",
"3. Define a noise model of the appropriate dimension.\n", "3. Define a noise model of the appropriate dimension.\n",
"3. Add the `CustomFactor` to a factor graph, specifying\n", "3. Add the `CustomFactor` to a factor graph, specifying\n",
" - the noise model\n", " - the noise model\n",
" - the keys of the variables it depends on\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", "\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>`." "Below is a simple example that mimics a `BetweenFactor<Pose2>`."
] ]
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 14, "execution_count": 2,
"id": "894bfaf2", "id": "894bfaf2",
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
@ -89,9 +133,9 @@
"import numpy as np\n", "import numpy as np\n",
"from gtsam import CustomFactor, noiseModel, Values, Pose2\n", "from gtsam import CustomFactor, noiseModel, Values, Pose2\n",
"\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", "\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", " \"\"\"\n",
" Error function that mimics a BetweenFactor\n", " Error function that mimics a BetweenFactor\n",
" :param this: reference to the current CustomFactor being evaluated\n", " :param this: reference to the current CustomFactor being evaluated\n",
@ -126,7 +170,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 15, "execution_count": 3,
"id": "c92caf2c", "id": "c92caf2c",
"metadata": {}, "metadata": {},
"outputs": [ "outputs": [
@ -163,12 +207,88 @@
}, },
{ {
"cell_type": "markdown", "cell_type": "markdown",
"id": "38c04012", "id": "d9b61f83",
"metadata": {}, "metadata": {},
"source": [ "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", "\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 # 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.