317 lines
12 KiB
Plaintext
317 lines
12 KiB
Plaintext
{
|
|
"cells": [
|
|
{
|
|
"cell_type": "markdown",
|
|
"id": "31f395c5",
|
|
"metadata": {},
|
|
"source": [
|
|
"# CustomFactor"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"id": "1a3591a2",
|
|
"metadata": {},
|
|
"source": [
|
|
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/nonlinear/doc/CustomFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 1,
|
|
"id": "5ccb48e4",
|
|
"metadata": {
|
|
"tags": [
|
|
"remove-cell"
|
|
],
|
|
"vscode": {
|
|
"languageId": "markdown"
|
|
}
|
|
},
|
|
"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"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"id": "10df70c9",
|
|
"metadata": {},
|
|
"source": [
|
|
"\n",
|
|
"## Overview\n",
|
|
"\n",
|
|
"The `CustomFactor` class allows users to define custom error functions and Jacobians, and while it can be used in C++, it is particularly useful for use with the python wrapper.\n",
|
|
"\n",
|
|
"## Custom Error Function\n",
|
|
"\n",
|
|
"The `CustomFactor` class allows users to define a custom error function. In C++ it is defined as below:\n",
|
|
"\n",
|
|
"```cpp\n",
|
|
"using JacobianVector = std::vector<Matrix>;\n",
|
|
"using CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, const JacobianVector *)>;\n",
|
|
"```\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",
|
|
"\n",
|
|
"## Usage in Python\n",
|
|
"\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"
|
|
]
|
|
},
|
|
{
|
|
"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": 2,
|
|
"id": "894bfaf2",
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"CustomFactor on 66, 77\n",
|
|
"isotropic dim=3 sigma=0.1\n",
|
|
"\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"import numpy as np\n",
|
|
"from gtsam import CustomFactor, noiseModel, Values, Pose2\n",
|
|
"\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]=None):\n",
|
|
" \"\"\"\n",
|
|
" Error function that mimics a BetweenFactor\n",
|
|
" :param this: reference to the current CustomFactor being evaluated\n",
|
|
" :param v: Values object\n",
|
|
" :param H: list of references to the Jacobian arrays\n",
|
|
" :return: the non-linear error\n",
|
|
" \"\"\"\n",
|
|
" key0 = this.keys()[0]\n",
|
|
" key1 = this.keys()[1]\n",
|
|
" gT1, gT2 = v.atPose2(key0), v.atPose2(key1)\n",
|
|
" error = measurement.localCoordinates(gT1.between(gT2))\n",
|
|
"\n",
|
|
" if H is not None:\n",
|
|
" result = gT1.between(gT2)\n",
|
|
" H[0] = -result.inverse().AdjointMap()\n",
|
|
" H[1] = np.eye(3)\n",
|
|
" return error\n",
|
|
"\n",
|
|
"# we use an isotropic noise model, and keys 66 and 77\n",
|
|
"noise_model = noiseModel.Isotropic.Sigma(3, 0.1)\n",
|
|
"custom_factor = CustomFactor(noise_model, [66, 77], error_func)\n",
|
|
"print(custom_factor)"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"id": "b72a8fc7",
|
|
"metadata": {},
|
|
"source": [
|
|
"Typically, you would not actually call methods of a custom factor directly: a nonlinear optimizer will call `linearize` in every nonlinear iteration. But if you wanted to, here is how you would do it:"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "code",
|
|
"execution_count": 3,
|
|
"id": "c92caf2c",
|
|
"metadata": {},
|
|
"outputs": [
|
|
{
|
|
"name": "stdout",
|
|
"output_type": "stream",
|
|
"text": [
|
|
"error = 0.0\n",
|
|
"Linearized JacobianFactor:\n",
|
|
" A[66] = [\n",
|
|
"\t-6.12323e-16, -10, -20;\n",
|
|
"\t10, -6.12323e-16, -20;\n",
|
|
"\t-0, -0, -10\n",
|
|
"]\n",
|
|
" A[77] = [\n",
|
|
"\t10, 0, 0;\n",
|
|
"\t0, 10, 0;\n",
|
|
"\t0, 0, 10\n",
|
|
"]\n",
|
|
" b = [ -0 -0 -0 ]\n",
|
|
" No noise model\n",
|
|
"\n"
|
|
]
|
|
}
|
|
],
|
|
"source": [
|
|
"values = Values()\n",
|
|
"values.insert(66, Pose2(1, 2, np.pi / 2))\n",
|
|
"values.insert(77, Pose2(-1, 4, np.pi))\n",
|
|
"\n",
|
|
"print(\"error = \", custom_factor.error(values))\n",
|
|
"print(\"Linearized JacobianFactor:\\n\", custom_factor.linearize(values))"
|
|
]
|
|
},
|
|
{
|
|
"cell_type": "markdown",
|
|
"id": "d9b61f83",
|
|
"metadata": {},
|
|
"source": [
|
|
"## Beware of Jacobians!\n",
|
|
"\n",
|
|
"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",
|
|
"```cpp\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",
|
|
"```cpp\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"
|
|
]
|
|
}
|
|
],
|
|
"metadata": {
|
|
"kernelspec": {
|
|
"display_name": "py312",
|
|
"language": "python",
|
|
"name": "python3"
|
|
},
|
|
"language_info": {
|
|
"codemirror_mode": {
|
|
"name": "ipython",
|
|
"version": 3
|
|
},
|
|
"file_extension": ".py",
|
|
"mimetype": "text/x-python",
|
|
"name": "python",
|
|
"nbconvert_exporter": "python",
|
|
"pygments_lexer": "ipython3",
|
|
"version": "3.12.6"
|
|
}
|
|
},
|
|
"nbformat": 4,
|
|
"nbformat_minor": 5
|
|
}
|