{ "cells": [ { "cell_type": "markdown", "id": "31f395c5", "metadata": {}, "source": [ "# CustomFactor" ] }, { "cell_type": "markdown", "id": "1a3591a2", "metadata": {}, "source": [ "\"Open" ] }, { "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;\n", "using CustomErrorFunction = std::function;\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`." ] }, { "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;\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 }