CustomFactor docs
Frank Dellaert
parent
50e40d00df
commit
ed6038d6f9
|
|
@ -5,45 +5,54 @@
|
|||
"id": "31f395c5",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"# CustomFactor Class Documentation\n",
|
||||
"\n",
|
||||
"*Disclaimer: This documentation was generated by AI and may require human revision for accuracy and completeness.*\n",
|
||||
"# 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": null,
|
||||
"id": "5ccb48e4",
|
||||
"metadata": {
|
||||
"tags": [
|
||||
"remove-cell"
|
||||
],
|
||||
"vscode": {
|
||||
"languageId": "markdown"
|
||||
}
|
||||
},
|
||||
"outputs": [],
|
||||
"source": [
|
||||
"%pip install --quiet gtsam-develop"
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "markdown",
|
||||
"id": "10df70c9",
|
||||
"metadata": {},
|
||||
"source": [
|
||||
"\n",
|
||||
"## Overview\n",
|
||||
"\n",
|
||||
"The `CustomFactor` class in GTSAM is a specialized type of factor used in factor graphs, which allows users to define custom error functions and Jacobians. This class is particularly useful when the standard factors provided by GTSAM do not meet specific application needs, allowing for greater flexibility and customization in defining the behavior of the factor.\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",
|
||||
"## Key Functionalities\n",
|
||||
"## Custom Error Function\n",
|
||||
"\n",
|
||||
"### Custom Error Function\n",
|
||||
"The `CustomFactor` class allows users to define a custom error function. In C++ it is defined as below:\n",
|
||||
"\n",
|
||||
"The `CustomFactor` class allows users to define a custom error function. This is a critical feature as it enables the modeling of specific constraints or measurements that are not covered by existing GTSAM factors. The error function typically represents the difference between predicted and observed measurements and is central to the optimization process in factor graphs.\n",
|
||||
"```c++\n",
|
||||
"using JacobianVector = std::vector<Matrix>;\n",
|
||||
"using CustomErrorFunction = std::function<Vector(const CustomFactor &, const Values &, const JacobianVector *)>;\n",
|
||||
"```\n",
|
||||
"\n",
|
||||
"### Jacobian Calculation\n",
|
||||
"\n",
|
||||
"In addition to the error function, users can define custom Jacobians. The Jacobian is the matrix of all first-order partial derivatives of a vector-valued function. In the context of `CustomFactor`, it represents the sensitivity of the error function with respect to the variables involved. This is crucial for optimization algorithms, which rely on gradient information to find the minimum of the error function.\n",
|
||||
"\n",
|
||||
"### Integration with Factor Graphs\n",
|
||||
"\n",
|
||||
"`CustomFactor` seamlessly integrates with GTSAM's factor graph framework. It can be added to a factor graph just like any other factor, participating in the graph optimization process. This integration ensures that custom factors can be used alongside standard factors, providing a flexible and powerful tool for solving complex estimation problems.\n",
|
||||
"\n",
|
||||
"## Mathematical Formulation\n",
|
||||
"\n",
|
||||
"### Error Function\n",
|
||||
"\n",
|
||||
"The error function $e(\\mathbf{x})$ is defined by the user and represents the discrepancy between the observed and predicted measurements. It is a vector-valued function:\n",
|
||||
"\n",
|
||||
"$$ e(\\mathbf{x}) = h(\\mathbf{x}) - \\mathbf{z} $$\n",
|
||||
"\n",
|
||||
"where $h(\\mathbf{x})$ is the predicted measurement based on the current estimate of the variables $\\mathbf{x}$, and $\\mathbf{z}$ is the observed measurement.\n",
|
||||
"\n",
|
||||
"### Jacobian\n",
|
||||
"\n",
|
||||
"The Jacobian matrix $J$ of the error function with respect to the variables $\\mathbf{x}$ is given by:\n",
|
||||
"\n",
|
||||
"$$ J = \\frac{\\partial e}{\\partial \\mathbf{x}} $$\n",
|
||||
"\n",
|
||||
"This matrix is used in the optimization process to update the estimates of the variables in a direction that reduces the error.\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",
|
||||
"\n",
|
||||
|
|
@ -51,13 +60,137 @@
|
|||
"\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. Add the `CustomFactor` to a factor graph, specifying the keys of the variables it depends on.\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",
|
||||
"\n",
|
||||
"By following these steps, users can leverage the flexibility of `CustomFactor` to incorporate custom measurements and constraints into their factor graph models, enhancing the capability of GTSAM to solve a wide range of estimation problems."
|
||||
"Below is a simple example that mimics a `BetweenFactor<Pose2>`."
|
||||
]
|
||||
},
|
||||
{
|
||||
"cell_type": "code",
|
||||
"execution_count": 14,
|
||||
"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)\n",
|
||||
"\n",
|
||||
"def error_func(this: CustomFactor, v: Values, H: list[np.ndarray]):\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": 15,
|
||||
"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": "38c04012",
|
||||
"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",
|
||||
"\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)."
|
||||
]
|
||||
}
|
||||
],
|
||||
"metadata": {},
|
||||
"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
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue