diff --git a/gtsam/nonlinear/doc/CustomFactor.ipynb b/gtsam/nonlinear/doc/CustomFactor.ipynb index 19adf335f..577efae07 100644 --- a/gtsam/nonlinear/doc/CustomFactor.ipynb +++ b/gtsam/nonlinear/doc/CustomFactor.ipynb @@ -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`." ] }, { "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;\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" ] } ], diff --git a/python/CustomFactors.md b/python/CustomFactors.md index 0a387bb4f..84b13bdfe 100644 --- a/python/CustomFactors.md +++ b/python/CustomFactors.md @@ -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(, , ) -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; -``` - -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.