12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #2080 from borglab/gpt-docs 

Doc generation with GPT
release/4.3a0
Frank Dellaert 2025-04-06 18:31:52 -04:00 committed by GitHub
parent d04959279e 2991ce67d7
commit 4226da3f0d
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
40 changed files with 4063 additions and 590 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
doc/examples.md Normal file
View File

@ -0,0 +1,3 @@
# Examples
This section contains python examples in interactive Python notebooks (`*.ipynb`). Python notebooks with an <img src="https://colab.research.google.com/assets/colab-badge.svg" alt="Open In Colab"/> button near the top can be opened in your browser, where you can run the files yourself and make edits to play with and understand GTSAM.

113
doc/expressions.md Normal file
View File

@ -0,0 +1,113 @@
# Expressions
## Motivation
GTSAM is an optimization library for objective functions expressed as a factor graph over a set of unknown variables. In the continuous case, the variables are typically vectors or elements on a manifold (such as the 3D rotation manifold). The factors compute vector-valued errors that need to be minimized, and are typically only connected to a handful of unknowns.
In the continuous case, the main optimization methods we have implemented are variants of Gauss-Newton non-linear optimization or conjugate gradient methods. Let us assume there are m factors over n unknowns. For either optimization method, we need to evaluate the sparse Jacobian matrix of the entire factor graph, which is a sparse block-matrix of m block-rows and n-block columns.
The sparse Jacobian is built up factor by factor, corresponding to the block-rows. A typical non-linear least-square term is $|h(x)-z|^2$ where $h(x)$ is a measurement function, which we need to be able to linearize as
$$
h(x) \approx h(x_0+dx)+H(x_0)dx
$$
Note the above is for vector unknowns, for Lie groups and manifold variables, see [doc/math.pdf](https://github.com/borglab/gtsam/blob/develop/doc/math.pdf) for details.
## Expressions
In many cases one can use GTSAM 4 Expressions to implement factors. Expressions are objects of type Expression<T>, and there are three main expression flavors:
- constants, e.g., `Expression<Point2> kExpr(Point2(3,4))`
- unknowns, e.g., `Expression<Point3> pExpr(123)` where 123 is a key.
- functions, e.g., `Expression<double> sumExpr(h, kexpr, pExpr)`
The latter case is an example of wrapping a binary measurement function `h`. To be able to wrap `h`, it needs to be able to compute its local derivatives, i.e., it has to have the signature
```c++
double h(const Point2& a, const Point3& b,
OptionalJacobian<1, 2> Ha, OptionalJacobian<1, 3> Hb)
```
In this case the output type 'T' is 'double', the two arguments have type Point2 and Point3 respectively, and the two remaining arguments provide a way to compute the function Jacobians, if needed. The templated type `OptionalJacobian<M,N>` behaves very much like `std::optional<Eigen::Matrix<double,M,N>`. If an actual matrix is passed in, the function is expected to treat it as an output argument in which to write the Jacobian for the result wrp. the corresponding input argument. *The matrix to write in will be allocated before the call.*
Expression constructors exist for both methods and functions with different arities. Note that an expression is templated with the output type T, not with the argument types. However, the constructor will infer the argument types from inspecting the signature of the function f, and will in this example expect two additional arguments of type Expression<Point2> and Expression<Point3>, respectively.
As an example, here is the constructor declaration for wrapping unary functions:
```c++
template<typename A>
Expression(typename UnaryFunction<A>::type function,
const Expression<A>& expression);
```
where (in this case) the function type is defined by
```c++
template<class A1>
struct UnaryFunction {
typedef boost::function<
T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> type;
};
```
## Some measurement function examples
An example of a simple unary function is `gtsam::norm3` in [Point3.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/geometry/Point3.cpp#L41):
```c++
double norm3(const Point3 & p, OptionalJacobian<1, 3> H = {}) {
double r = sqrt(p.x() * p.x() + p.y() * p.y() + p.z() * p.z());
if (H) *H << p.x() / r, p.y() / r, p.z() / r;
return r;
}
```
The key new concept here is OptionalJacobian, which acts like a std::optional: if it evaluates to true, you should write the Jacobian of the function in it. It acts as a fixed-size Eigen matrix.
As we said above, expressions also support binary functions, ternary functions, and methods. An example of a binary function is 'Point3::cross':
```c++
Point3 cross(const Point3 &p, const Point3 & q,
OptionalJacobian<3, 3> H1 = {}, OptionalJacobian<3, 3> H2 = {}) {
if (H1) *H1 << skewSymmetric(-q.x(), -q.y(), -q.z());
if (H2) *H2 << skewSymmetric(p.x(), p.y(), p.z());
return Point3(p.y() * q.z() - p.z() * q.y(), p.z() * q.x() - p.x() * q.z(), p.x() * q.y() - p.y() * q.x());
}
```
Example of using cross:
```c++
using namespace gtsam;
Matrix3 H1, H2;
Point3 p(1,2,3), q(4,5,6), r = cross(p,q,H1,H2);
```
## Using Expressions for Inference
The way expressions are used is by creating unknown Expressions for the unknown variables we are optimizing for:
```c++
Expression<Point3> x(‘x’,1);
auto h = Expression<Point3>(& norm3, x);
```
For convenient creation of factors with expressions, we provide a new factor graph type `ExpressionFactorGraph`, which is just a `NonlinearFactorGraph` with an extra method addExpressionFactor(h, z, n) that takes a measurement expression h, an actual measurement z, and a measurement noise model R. With this, we can add a GTSAM nonlinear factor $|h(x)-z|^2$ to a `NonlinearFactorGraph` by
```c++
graph.addExpressionFactor(h, z, R)
```
In the above, the unknown in the example can be retrieved by the `gtsam::Symbol(‘x’,1)`, which evaluates to a uint64 identifier.
## Composing Expressions
The key coolness behind expressions, however, is that you can compose them into expression trees, as long as the leaves know how to do their own derivatives:
```c++
Expression<Point3> x1(‘x’1), x2(‘x’,2);
auto h = Expression<Point3>(& cross, x1, x2);
auto g = Expression<Point3>(& norm3, h);
```
Because we typedef Point3_ to Expression<Point3>, we can write this very concisely as
```c++
auto g = Point3_(& norm3, Point3_(& cross, x1(‘x’1), x2(‘x’,2)));
```
## PoseSLAM Example
Using expressions, it is simple to quickly create a factor graph corresponding to a PoseSLAM problem, where our only measurements are relative poses between a series of unknown 2D or 3D poses. The following code snippet from [Pose2SLAMExampleExpressions.cpp](https://github.com/borglab/gtsam/blob/develop/examples/Pose2SLAMExampleExpressions.cpp) is used to create a simple Pose2 example (where the robot is moving on a plane):
```c++
1 ExpressionFactorGraph graph;
2 Expression<Pose2> x1(1), x2(2), x3(3), x4(4), x5(5);
3 graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise);
4 graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0 ), model);
5 graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model);
6 graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model);
7 graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model);
8 graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model);
```
This is what is going on:
- In line 1, we create an empty factor graph.
- In line 2 we create the 5 unknown poses, of type `Expression<Pose2>`, with keys 1 to 5. These are what we will optimize over.
- Line 3 then creates a simple factor that gives a prior on `x1` (the first argument), namely that it is at the origin `Pose2(0, 0, 0)` (the second argument), with a particular probability density given by `priorNoise` (the third argument).
- Lines 4-7 adds factors for the odometry constraints, i.e., the movement between successive poses of the robot. The function `between(t1,t2)` is implemented in [nonlinear/expressions.h](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/expressions.h) and is equivalent to calling the constructor Expression<T>(traits<T>::Between, t1, t2).
- Finally, line 8 creates a loop closure constraint between poses x2 and x5.
Another good example of its use is in
[SFMExampleExpressions.cpp](https://github.com/borglab/gtsam/blob/develop/examples/SFMExampleExpressions.cpp).

138
doc/generating/gpt_generate.py Normal file
View File

@ -0,0 +1,138 @@
"""
GTSAM Copyright 2010-2025, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Author: Porter Zach
This script generates interactive Python notebooks (.ipynb) that document GTSAM
header files. Since inserting the text of the file directly into the prompt
might be too many tokens, it retrieves the header file content from the GTSAM
GitHub repository. It then sends it to OpenAI's API for processing, and saves
the generated documentation as a Jupyter notebook.
Functions:
is_url_valid(url: str) -> bool:
Verifies that the supplied URL does not return a 404.
save_ipynb(text: str, file_path: str) -> str:
Saves the provided text to a single Markdown cell in a new .ipynb file.
generate_ipynb(file_path: str, openai_client):
Generates an interactive Python notebook for the given GTSAM header file
by sending a request to OpenAI's API and saving the response.
Usage:
Run the script with paths to GTSAM header files as arguments. For example:
python gpt_generate.py gtsam/geometry/Pose3.h
"""
import os
import time
import requests
import argparse
import nbformat as nbf
from openai import OpenAI
_output_folder = "output"
_gtsam_gh_base = "https://raw.githubusercontent.com/borglab/gtsam/refs/heads/develop/"
_asst_id = "asst_na7wYBtXyGU0x5t2RdcnpxzP"
_request_text = "Document the file found at {}."
def is_url_valid(url):
"""Verify that the supplied URL does not return a 404."""
try:
response = requests.head(url, allow_redirects=True)
return response.status_code != 404
except requests.RequestException:
return False
def save_ipynb(text: str, file_path: str):
"""Save text to a single Markdown cell in a new .ipynb file."""
script_dir = os.path.dirname(os.path.abspath(__file__))
output_dir = os.path.join(script_dir, _output_folder)
os.makedirs(output_dir, exist_ok=True)
output_file = os.path.splitext(os.path.basename(file_path))[0] + ".ipynb"
output_full_path = os.path.join(output_dir, output_file)
nb = nbf.v4.new_notebook()
new_cell = nbf.v4.new_markdown_cell(text)
nb['cells'].append(new_cell)
with open(output_full_path, 'w', encoding='utf-8') as file:
nbf.write(nb, file)
return output_file
def generate_ipynb(file_path: str, openai_client):
"""Generate an interactive Python notebook for the given GTSAM header file.
Args:
file_path (str): The fully-qualified path from the root of the gtsam
repository to the header file that will be documented.
openai_client (openai.OpenAI): The OpenAI client to use.
"""
# Create the URL to get the header file from.
url = _gtsam_gh_base + file_path
if not is_url_valid(url):
print(f"{url} was not found on the server, or an error occurred.")
return
print(f"Sending request to OpenAI to document {url}.")
# Create a new thread and send the request
thread = openai_client.beta.threads.create()
openai_client.beta.threads.messages.create(
thread_id=thread.id, role="user", content=_request_text.format(url))
run = openai_client.beta.threads.runs.create(thread_id=thread.id,
assistant_id=_asst_id)
print("Waiting for the assistant to process the request...")
# Wait for request to be processed
while True:
run_status = openai_client.beta.threads.runs.retrieve(
thread_id=thread.id, run_id=run.id)
if run_status.status == "completed":
break
time.sleep(2)
print("Request processed. Retrieving response...")
# Fetch messages
messages = openai_client.beta.threads.messages.list(thread_id=thread.id)
# Retrieve response text and strip ```markdown ... ```
text = messages.data[0].content[0].text.value.strip('`').strip('markdown')
# Write output to file
output_filename = save_ipynb(text, file_path)
print(f"Response retrieved. Find output in {output_filename}.")
if __name__ == "__main__":
parser = argparse.ArgumentParser(
prog="gpt_generate",
description=
"Generates .ipynb documentation files given paths to GTSAM header files."
)
parser.add_argument(
"file_paths",
nargs='+',
help=
"The paths to the header files from the root gtsam directory, e.g. 'gtsam/geometry/Pose3.h'."
)
args = parser.parse_args()
# Retrieves API key from environment variable OPENAI_API_KEY
client = OpenAI()
for file_path in args.file_paths:
generate_ipynb(file_path, client)

0
gtsam/user_guide.md → doc/user_guide.md
View File

11
gtsam/geometry/geometry.i
View File

@ -1075,9 +1075,14 @@ class PinholeCamera {
pair<gtsam::Point2, bool> projectSafe(const gtsam::Point3& pw) const;
gtsam::Point2 project(const gtsam::Point3& point);
gtsam::Point2 project(const gtsam::Point3& point,
Eigen::Ref<Eigen::MatrixXd> Dpose,
Eigen::Ref<Eigen::MatrixXd> Dpoint,
Eigen::Ref<Eigen::MatrixXd> Dcal);
Eigen::Ref<Eigen::MatrixXd> Dpose);
gtsam::Point2 project(const gtsam::Point3& point,
Eigen::Ref<Eigen::MatrixXd> Dpose,
Eigen::Ref<Eigen::MatrixXd> Dpoint);
gtsam::Point2 project(const gtsam::Point3& point,
Eigen::Ref<Eigen::MatrixXd> Dpose,
Eigen::Ref<Eigen::MatrixXd> Dpoint,
Eigen::Ref<Eigen::MatrixXd> Dcal);
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
gtsam::Point3 backproject(const gtsam::Point2& p, double depth,
Eigen::Ref<Eigen::MatrixXd> Dresult_dpose,

13
gtsam/nonlinear/BatchFixedLagSmoother.h
View File

@ -33,9 +33,18 @@ public:
/// Typedef for a shared pointer to an Incremental Fixed-Lag Smoother
typedef std::shared_ptr<BatchFixedLagSmoother> shared_ptr;
/** default constructor */
/**
* Construct with parameters
*
* @param smootherLag The length of the smoother lag. Any variable older than this amount will be marginalized out.
* @param parameters The L-M optimization parameters
* @param enforceConsistency A flag indicating if the optimizer should enforce probabilistic consistency by maintaining the
* linearization point of all variables involved in linearized/marginal factors at the edge of the
* smoothing window.
*/
BatchFixedLagSmoother(double smootherLag = 0.0, const LevenbergMarquardtParams& parameters = LevenbergMarquardtParams(), bool enforceConsistency = true) :
FixedLagSmoother(smootherLag), parameters_(parameters), enforceConsistency_(enforceConsistency) { }
FixedLagSmoother(smootherLag), parameters_(parameters), enforceConsistency_(enforceConsistency) {
}
/** destructor */
~BatchFixedLagSmoother() override {}

59
gtsam/nonlinear/doc/BatchFixedLagSmoother.ipynb Normal file
View File

@ -0,0 +1,59 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "283174f8",
"metadata": {},
"source": [
"# BatchFixedLagSmoother\n",
"\n",
"## Overview\n",
"\n",
"The `IncrementalFixedLagSmoother` is a [FixedLagSmoother](FixedLagSmoother.ipynb) that uses [LevenbergMarquardtOptimizer](LevenbergMarquardtOptimizer.ipynb) for batch optimization.\n",
"\n",
"This fixed lag smoother will **batch-optimize** at every iteration, but warm-started from the last estimate."
]
},
{
"cell_type": "markdown",
"id": "92b4f851",
"metadata": {},
"source": [
"## API\n",
"\n",
"### Constructor\n",
"\n",
"You construct a `BatchFixedLagSmoother` object with the following parameters:\n",
"\n",
"- **smootherLag**: The length of the smoother lag. Any variable older than this amount will be marginalized out. *(Default: 0.0)*\n",
"- **parameters**: The Levenberg-Marquardt optimization parameters. *(Default: `LevenbergMarquardtParams()`)* \n",
"- **enforceConsistency**: A flag indicating whether the optimizer should enforce probabilistic consistency by maintaining the linearization point of all variables involved in linearized/marginal factors at the edge of the smoothing window. *(Default: `true`)*\n",
"\n",
"### Smoothing and Optimization\n",
"\n",
"- **update**: This method is the core of the `BatchFixedLagSmoother`. It processes new factors and variables, updating the current estimate of the state. The update method also manages the marginalization of variables that fall outside the fixed lag window.\n",
"\n",
"### Computational Considerations\n",
"\n",
"Every call to `update` triggers a batch LM optimization: use the parameters to control the convergence thresholds to bound computation to fit within your application."
]
},
{
"cell_type": "markdown",
"id": "4a2bdd3e",
"metadata": {},
"source": [
"## Internals\n",
"\n",
"- **marginalize**: This function handles the marginalization of variables that are no longer within the fixed lag window. Marginalization is a crucial step in maintaining the size of the factor graph, ensuring that only relevant variables are kept for optimization.\n"
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

316
gtsam/nonlinear/doc/CustomFactor.ipynb Normal file
View File

@ -0,0 +1,316 @@
{
"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
}

80
gtsam/nonlinear/doc/DoglegOptimizer.ipynb Normal file
View File

@ -0,0 +1,80 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "f851cef5",
"metadata": {},
"source": [
"# DoglegOptimizer\n",
"\n",
"## Overview\n",
"\n",
"The `DoglegOptimizer` class in GTSAM is a specialized optimization algorithm designed for solving nonlinear least squares problems. It implements the Dogleg method, which is a hybrid approach combining the steepest descent and Gauss-Newton methods.\n",
"\n",
"The Dogleg method is characterized by its use of two distinct steps:\n",
"\n",
"1. **Cauchy Point**: The steepest descent direction, calculated as:\n",
" $$ p_u = -\\alpha \\nabla f(x) $$\n",
" where $\\alpha$ is a scalar step size.\n",
"\n",
"2. **Gauss-Newton Step**: The solution to the linearized problem, providing a more accurate but computationally expensive step:\n",
" $$ p_{gn} = -(J^T J)^{-1} J^T r $$\n",
" where $J$ is the Jacobian matrix and $r$ is the residual vector.\n",
"\n",
"The Dogleg step, $p_{dl}$, is a combination of these two steps, determined by the trust region radius $\\Delta$.\n",
"\n",
"Key features:\n",
"\n",
"- **Hybrid Approach**: Combines the strengths of both the steepest descent and Gauss-Newton methods.\n",
"- **Trust Region Method**: Utilizes a trust region to determine the step size, balancing between the accuracy of Gauss-Newton and the robustness of steepest descent.\n",
"- **Efficient for Nonlinear Problems**: Designed to handle complex nonlinear least squares problems effectively."
]
},
{
"cell_type": "markdown",
"id": "758e347b",
"metadata": {},
"source": [
"## Key Methods\n",
"\n",
"Please see the base class [NonlinearOptimizer.ipynb](NonlinearOptimizer.ipynb).\n",
"\n",
"## Parameters\n",
"\n",
"The `DoglegParams` class defines parameters specific to Powell's Dogleg optimization algorithm:\n",
"\n",
"| Parameter | Description |\n",
"|-----------|-------------|\n",
"| `deltaInitial` | Initial trust region radius that controls step size (default: 1.0) |\n",
"| `verbosityDL` | Controls algorithm-specific diagnostic output (options: SILENT, VERBOSE) |\n",
"\n",
"These parameters complement the standard optimization parameters inherited from `NonlinearOptimizerParams`, which include:\n",
"\n",
"- Maximum iterations\n",
"- Relative and absolute error thresholds\n",
"- Error function verbosity\n",
"- Linear solver type\n",
"\n",
"Powell's Dogleg algorithm combines Gauss-Newton and gradient descent approaches within a trust region framework. The `deltaInitial` parameter defines the initial size of this trust region, which adaptively changes during optimization based on how well the linear approximation matches the nonlinear function.\n",
"\n",
"## Usage Considerations\n",
"\n",
"- **Initial Guess**: The performance of the Dogleg optimizer can be sensitive to the initial guess. A good initial estimate can significantly speed up convergence.\n",
"- **Parameter Tuning**: The choice of the initial trust region radius and other parameters can affect the convergence rate and stability of the optimization.\n",
"\n",
"## Files\n",
"\n",
"- [DoglegOptimizer.h](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/DoglegOptimizer.h)\n",
"- [DoglegOptimizer.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/DoglegOptimizer.cpp)\n",
"- [DoglegParams.h](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/DoglegParams.h)"
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

46
gtsam/nonlinear/doc/ExpressionFactor.ipynb Normal file
View File

@ -0,0 +1,46 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "59407eaf",
"metadata": {},
"source": [
"# ExpressionFactor\n",
"\n",
"## Overview\n",
"\n",
"The `ExpressionFactor` class in GTSAM is a template class designed to work with factor graphs in the context of nonlinear optimization. It represents a factor that can be constructed from a [GTSAM expression](../../../doc/expressions.md), allowing for flexible and efficient computation of error terms in optimization problems.\n",
"\n",
"The `ExpressionFactor` class allows users to define factors based on expressions in C++, that use (reverse) automatic differentiation to compute their Jacobians.\n",
"\n",
"## Main Methods\n",
"\n",
"### Constructor\n",
"\n",
"The `ExpressionFactor` class provides a constructor that allows for the initialization of the factor with a specific expression and measurement:\n",
"\n",
"```cpp\n",
" /**\n",
" * Constructor: creates a factor from a measurement and measurement function\n",
" * @param noiseModel the noise model associated with a measurement\n",
" * @param measurement actual value of the measurement, of type T\n",
" * @param expression predicts the measurement from Values\n",
" * The keys associated with the factor, returned by keys(), are sorted.\n",
" */\n",
" ExpressionFactor(const SharedNoiseModel& noiseModel, //\n",
" const T& measurement, const Expression<T>& expression)\n",
" : NoiseModelFactor(noiseModel), measured_(measurement) {\n",
" initialize(expression);\n",
" }\n",
"```"
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

35
gtsam/nonlinear/doc/ExpressionFactorGraph.ipynb Normal file
View File

@ -0,0 +1,35 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "a1c00a8c",
"metadata": {},
"source": [
"# ExpressionFactorGraph \n",
"\n",
"## Overview\n",
"\n",
"The `ExpressionFactorGraph` class in GTSAM is a specialized factor graph designed to work with expressions. It extends the capabilities of a standard factor graph by allowing factors created from [GTSAM expressions](../../../doc/expressions.md), that implement automatic differentiation. It creates [ExpressionFactors](ExpressionFactor.ipynb).\n",
"\n",
"### Adding Expression Factors\n",
"\n",
"use **addExpressionFactor**: This method allows the user to add a new factor to the graph based on a symbolic expression. The expression defines the relationship between the variables involved in the factor.\n",
"```c++\n",
" template<typename T>\n",
" void addExpressionFactor(const Expression<T>& h, const T& z,\n",
" const SharedNoiseModel& R) {\n",
" using F = ExpressionFactor<T>;\n",
" push_back(std::allocate_shared<F>(Eigen::aligned_allocator<F>(), R, z, h));\n",
" }\n",
"```"
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

79
gtsam/nonlinear/doc/ExtendedKalmanFilter.ipynb Normal file
View File

@ -0,0 +1,79 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "93869c17",
"metadata": {},
"source": [
"# ExtendedKalmanFilter\n",
"\n",
"## Overview\n",
"\n",
"The `ExtendedKalmanFilter` class in GTSAM is an implementation of the [Extended Kalman Filter (EKF)](https://en.wikipedia.org/wiki/Extended_Kalman_filter), which is a powerful tool for estimating the state of a nonlinear dynamic system.\n",
"\n",
"See also [this notebook](../../../python/gtsam/examples/easyPoint2KalmanFilter.ipynb) for the python version of the C++ example below."
]
},
{
"cell_type": "markdown",
"id": "161c36eb",
"metadata": {},
"source": [
"## Using the ExtendedKalmanFilter Class\n",
"\n",
"The `ExtendedKalmanFilter` class in GTSAM provides a flexible way to implement Kalman filtering using factor graphs. Here's a step-by-step guide based on the example provided in [easyPoint2KalmanFilter.cpp](https://github.com/borglab/gtsam/blob/develop/examples/easyPoint2KalmanFilter.cpp):\n",
"\n",
"### Steps to Use the ExtendedKalmanFilter\n",
"\n",
"1. **Initialize the Filter**:\n",
" - Define the initial state (e.g., position) and its covariance.\n",
" - Create a key for the initial state.\n",
" - Instantiate the `ExtendedKalmanFilter` object with the initial state and covariance.\n",
"\n",
" ```cpp\n",
" Point2 x_initial(0.0, 0.0);\n",
" SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));\n",
" Symbol x0('x', 0);\n",
" ExtendedKalmanFilter<Point2> ekf(x0, x_initial, P_initial);\n",
" ```\n",
"\n",
"2. Predict the Next State:\n",
"\n",
" - Define the motion model using a BetweenFactor.\n",
" - Predict the next state using the predict method.\n",
" ```cpp\n",
" Symbol x1('x', 1);\n",
" Point2 difference(1, 0);\n",
" SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1), true);\n",
" BetweenFactor<Point2> factor1(x0, x1, difference, Q);\n",
" Point2 x1_predict = ekf.predict(factor1);\n",
" ```\n",
"\n",
"3. Update the State with Measurements:\n",
" - Define the measurement model using a PriorFactor.\n",
" - Update the state using the update method.\n",
" ```cpp\n",
" Point2 z1(1.0, 0.0);\n",
" SharedDiagonal R = noiseModel::Diagonal::Sigmas(Vector2(0.25, 0.25), true);\n",
" PriorFactor<Point2> factor2(x1, z1, R);\n",
" Point2 x1_update = ekf.update(factor2);\n",
" ```\n",
"4. Repeat for Subsequent Time Steps:\n",
"\n",
" - Repeat the prediction and update steps for subsequent states and measurements.\n",
"\n",
"## Example Use Case\n",
"This example demonstrates tracking a moving 2D point using a simple linear motion model and position measurements. The ExtendedKalmanFilter class allows for flexible modeling of both the motion and measurement processes using GTSAM's factor graph framework.\n",
"\n",
"For the full implementation, see the [easyPoint2KalmanFilter.cpp](https://github.com/borglab/gtsam/blob/develop/examples/easyPoint2KalmanFilter.cpp) file.\n"
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

44
gtsam/nonlinear/doc/FixedLagSmoother.ipynb Normal file
View File

@ -0,0 +1,44 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "cdd2fdc5",
"metadata": {},
"source": [
"# FixedLagSmoother\n",
"\n",
"## Overview\n",
"\n",
"The `FixedLagSmoother` class is the base class for [BatchFixedLagSmoother](BatchFixedLagSmoother.ipynb) and [IncrementalFixedLagSmoother](IncrementalFixedLagSmoother.ipynb).\n",
"\n",
"It provides an API for fixed-lag smoothing in nonlinear factor graphs. It maintains a sliding window of the most recent variables and marginalizes out older variables. This is particularly useful in real-time applications where memory and computational efficiency are critical."
]
},
{
"cell_type": "markdown",
"id": "8d372784",
"metadata": {},
"source": [
"## Mathematical Formulation\n",
"\n",
"In fixed-lag smoothing the objective is to estimate the state $\\mathbf{x}_t$ given all measurements up to time $t$, but only retaining a fixed window of recent states. The optimization problem can be expressed as:\n",
"$$\n",
"\\min_{\\mathbf{x}_{t-L:t}} \\sum_{i=1}^{N} \\| \\mathbf{h}_i(\\mathbf{x}_{t-L:t}) - \\mathbf{z}_i \\|^2\n",
"$$\n",
"where $L$ is the fixed lag, $\\mathbf{h}_i$ are the measurement functions, and $\\mathbf{z}_i$ are the measurements.\n",
"In practice, the functions $\\mathbf{h}_i$ depend only on a subset of the state variables $\\mathbf{X}_i$, and the optimization is performed over a set of $N$ *factors* $\\phi_i$ instead:\n",
"$$\n",
"\\min_{\\mathbf{x}_{t-L:t}} \\sum_{i=1}^{N} \\| \\phi_i(\\mathbf{X}_i; \\mathbf{z}_i) \\|^2\n",
"$$\n",
"The API below allows the user to add new factors at every iteration, which will be automatically pruned after they no longer depend on any variables in the lag."
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

63
gtsam/nonlinear/doc/GaussNewtonOptimizer.ipynb Normal file
View File

@ -0,0 +1,63 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "6463d580",
"metadata": {},
"source": [
"# GaussNewtonOptimizer\n",
"\n",
"## Overview\n",
"\n",
"The `GaussNewtonOptimizer` class in GTSAM is designed to optimize nonlinear factor graphs using the Gauss-Newton algorithm. This class is particularly suited for problems where the cost function can be approximated well by a quadratic function near the minimum. The Gauss-Newton method is an iterative optimization technique that updates the solution by linearizing the nonlinear system at each iteration.\n",
"\n",
"The Gauss-Newton algorithm is based on the idea of linearizing the nonlinear residuals $r(x)$ around the current estimate $x_k$. The update step is derived from solving the normal equations:\n",
"\n",
"$$ J(x_k)^T J(x_k) \\Delta x = -J(x_k)^T r(x_k) $$\n",
"\n",
"where $J(x_k)$ is the Jacobian of the residuals with respect to the variables. The solution $\\Delta x$ is used to update the estimate:\n",
"\n",
"$$ x_{k+1} = x_k + \\Delta x $$\n",
"\n",
"This process is repeated iteratively until convergence.\n",
"\n",
"Key features:\n",
"\n",
"- **Iterative Optimization**: The optimizer refines the solution iteratively by linearizing the nonlinear system around the current estimate.\n",
"- **Convergence Control**: It provides mechanisms to control the convergence through parameters such as maximum iterations and relative error tolerance.\n",
"- **Integration with GTSAM**: Seamlessly integrates with GTSAM's factor graph framework, allowing it to be used with various types of factors and variables.\n",
"\n",
"## Key Methods\n",
"\n",
"Please see the base class [NonlinearOptimizer.ipynb](NonlinearOptimizer.ipynb).\n",
"\n",
"## Parameters\n",
"\n",
"The Gauss-Newton optimizer uses the standard optimization parameters inherited from `NonlinearOptimizerParams`, which include:\n",
"\n",
"- Maximum iterations\n",
"- Relative and absolute error thresholds\n",
"- Error function verbosity\n",
"- Linear solver type\n",
"\n",
"## Usage Considerations\n",
"\n",
"- **Initial Guess**: The quality of the initial guess can significantly affect the convergence and performance of the Gauss-Newton optimizer.\n",
"- **Non-convexity**: Since the method relies on linear approximations, it may struggle with highly non-convex problems or those with poor initial estimates.\n",
"- **Performance**: The Gauss-Newton method is generally faster than other nonlinear optimization methods like Levenberg-Marquardt for problems that are well-approximated by a quadratic model near the solution.\n",
"\n",
"## Files\n",
"\n",
"- [GaussNewtonOptimizer.h](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/GaussNewtonOptimizer.h)\n",
"- [GaussNewtonOptimizer.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/GaussNewtonOptimizer.cpp)"
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

79
gtsam/nonlinear/doc/GncOptimizer.ipynb Normal file
View File

@ -0,0 +1,79 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "c950beef",
"metadata": {},
"source": [
"# GncOptimizer\n",
"\n",
"## Overview\n",
"\n",
"The `GncOptimizer` class in GTSAM is designed to perform robust optimization using Graduated Non-Convexity (GNC). This method is particularly useful in scenarios where the optimization problem is affected by outliers. The GNC approach gradually transitions from a convex approximation of the problem to the original non-convex problem, thereby improving robustness and convergence.\n",
"\n",
"The `GncOptimizer` leverages a robust cost function $\\rho(e)$, where $e$ is the error term. The goal is to minimize the sum of these robust costs over all measurements:\n",
"\n",
"$$\n",
"\\min_x \\sum_i \\rho(e_i(x))\n",
"$$\n",
"\n",
"In the context of GNC, the robust cost function is gradually transformed from a convex approximation to the original non-convex form. This transformation is controlled by a parameter $\\mu$, which is adjusted during the optimization process:\n",
"\n",
"$$\n",
"\\rho_\\mu(e) = \\frac{1}{\\mu} \\rho(\\mu e)\n",
"$$\n",
"\n",
"As $\\mu$ increases, the function $\\rho_\\mu(e)$ transitions from a convex to a non-convex shape, allowing the optimizer to handle outliers effectively.\n",
"\n",
"Key features:\n",
"\n",
"- **Robust Optimization**: The GncOptimizer is specifically tailored to handle optimization problems with outliers, using a robust cost function that can mitigate their effects.\n",
"- **Graduated Non-Convexity**: This technique allows the optimizer to start with a convex problem and gradually transform it into the original non-convex problem, which helps in avoiding local minima.\n",
"- **Customizable Parameters**: Users can adjust various parameters to control the behavior of the optimizer, such as the type of robust loss function and the parameters governing the GNC process.\n",
"\n",
"## Key Methods\n",
"\n",
"Please see the base class [NonlinearOptimizer.ipynb](NonlinearOptimizer.ipynb).\n",
"\n",
"## Parameters\n",
"\n",
"The `GncParams` class defines parameters specific to the GNC optimization algorithm:\n",
"\n",
"| Parameter | Type | Default Value | Description |\n",
"|-----------|------|---------------|-------------|\n",
"| lossType | GncLossType | TLS | Type of robust loss function (GM = Geman McClure or TLS = Truncated least squares) |\n",
"| maxIterations | size_t | 100 | Maximum number of iterations |\n",
"| muStep | double | 1.4 | Multiplicative factor to reduce/increase mu in GNC |\n",
"| relativeCostTol | double | 1e-5 | Threshold for relative cost change to stop iterating |\n",
"| weightsTol | double | 1e-4 | Threshold for weights being close to binary to stop iterating (TLS only) |\n",
"| verbosity | Verbosity enum | SILENT | Verbosity level (options: SILENT, SUMMARY, MU, WEIGHTS, VALUES) |\n",
"| knownInliers | IndexVector | Empty | Slots in factor graph for measurements known to be inliers |\n",
"| knownOutliers | IndexVector | Empty | Slots in factor graph for measurements known to be outliers |\n",
"\n",
"These parameters complement the standard optimization parameters inherited from `NonlinearOptimizerParams`, which include:\n",
"\n",
"- Maximum iterations\n",
"- Relative and absolute error thresholds\n",
"- Error function verbosity\n",
"- Linear solver type\n",
"\n",
"## Usage Considerations\n",
"\n",
"- **Outlier Rejection**: The `GncOptimizer` is particularly effective in scenarios with significant outlier presence, such as SLAM or bundle adjustment problems.\n",
"- **Parameter Tuning**: Proper tuning of the GNC parameters is crucial for achieving optimal performance. Users should experiment with different settings to find the best configuration for their specific problem.\n",
"\n",
"## Files\n",
"\n",
"- [GncOptimizer.h](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/GncOptimizer.h)\n",
"- [GncParams.h](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/GncParams.h)"
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

56
gtsam/nonlinear/doc/ISAM2.ipynb Normal file
View File

@ -0,0 +1,56 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "867a20bc",
"metadata": {},
"source": [
"# ISAM2\n",
"\n",
"## Overview\n",
"\n",
"The `ISAM2` class in GTSAM is an incremental smoothing and mapping algorithm that efficiently updates the solution to a nonlinear optimization problem as new measurements are added. This class is particularly useful in applications such as SLAM (Simultaneous Localization and Mapping) where real-time performance is crucial. \n",
"\n",
"The algorithm is described in the 2012 IJJR paper by {cite:t}`http://dx.doi.org/10.1177/0278364911430419`. For background, also see the more recent booklet by {cite:t}`https://doi.org/10.1561/2300000043`.\n",
"\n",
"## Key Features\n",
"\n",
"- **Incremental Updates**: `ISAM2` allows for incremental updates to the factor graph, avoiding the need to solve the entire problem from scratch with each new measurement.\n",
"- **Nonlinear Optimization**: Capable of handling nonlinear systems, leveraging iterative optimization techniques to refine estimates.\n",
"- **Efficient Variable Reordering**: Dynamically reorders variables to maintain sparsity and improve computational efficiency."
]
},
{
"cell_type": "markdown",
"id": "9ce0ec12",
"metadata": {},
"source": [
"## Main Methods\n",
"\n",
"### Initialization and Configuration\n",
"\n",
"- **ISAM2 Constructor**: Initializes the `ISAM2` object with optional parameters for configuring the behavior of the algorithm, such as relinearization thresholds and ordering strategies.\n",
"\n",
"### Updating the Graph\n",
"\n",
"- **update**: Incorporates new factors and variables into the existing factor graph. This method performs the core incremental update, refining the solution based on new measurements.\n",
"\n",
"### Accessing Results\n",
"\n",
"- **calculateEstimate**: Retrieves the current estimate of the variables in the factor graph. This method can be called with specific variable keys to obtain their estimates.\n",
"- **marginalCovariance**: Computes the marginal covariance of a specified variable, providing insight into the uncertainty of the estimate.\n",
"\n",
"### Advanced Features\n",
"\n",
"- **getFactorsUnsafe**: Provides access to the internal factor graph, allowing for advanced manipulations and custom analysis."
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

23
gtsam/nonlinear/doc/IncrementalFixedLagSmoother.ipynb Normal file
View File

@ -0,0 +1,23 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "cdd2fdc5",
"metadata": {},
"source": [
"# IncrementalFixedLagSmoother\n",
"\n",
"## Overview\n",
"\n",
"The `IncrementalFixedLagSmoother` is a [FixedLagSmoother](FixedLagSmoother.ipynb) that uses [iSAM2](iSAM2.ipynb) for incremental inference.\n"
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

87
gtsam/nonlinear/doc/LevenbergMarquardtOptimizer.ipynb Normal file
View File

@ -0,0 +1,87 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "29642bb2",
"metadata": {},
"source": [
"# LevenbergMarquardtOptimizer\n",
"\n",
"## Overview\n",
"\n",
"The `LevenbergMarquardtOptimizer` class in GTSAM is a specialized optimizer that implements the Levenberg-Marquardt algorithm. This algorithm is a popular choice for solving non-linear least squares problems, which are common in various applications such as computer vision, robotics, and machine learning.\n",
"\n",
"The Levenberg-Marquardt algorithm is an iterative technique that interpolates between the Gauss-Newton algorithm and the method of gradient descent. It is particularly useful for optimizing problems where the solution is expected to be near the initial guess.\n",
"\n",
"The Levenberg-Marquardt algorithm seeks to minimize a cost function $F(x)$ of the form:\n",
"\n",
"$$\n",
"F(x) = \\frac{1}{2} \\sum_{i=1}^{m} r_i(x)^2\n",
"$$\n",
"\n",
"where $r_i(x)$ are the residuals. The update rule for the algorithm is given by:\n",
"\n",
"$$\n",
"x_{k+1} = x_k - (J^T J + \\lambda I)^{-1} J^T r\n",
"$$\n",
"\n",
"Here, $J$ is the Jacobian matrix of the residuals, $\\lambda$ is the damping parameter, and $I$ is the identity matrix.\n",
"\n",
"Key features:\n",
"\n",
"- **Non-linear Optimization**: The class is designed to handle non-linear optimization problems efficiently.\n",
"- **Damping Mechanism**: It incorporates a damping parameter to control the step size, balancing between the Gauss-Newton and gradient descent methods.\n",
"- **Iterative Improvement**: The optimizer iteratively refines the solution, reducing the error at each step.\n",
"\n",
"## Key Methods\n",
"\n",
"Please see the base class [NonlinearOptimizer.ipynb](NonlinearOptimizer.ipynb).\n",
"\n",
"## Parameters\n",
"\n",
"The `LevenbergMarquardtParams` class defines parameters specific to this optimization algorithm:\n",
"\n",
"| Parameter | Type | Default Value | Description |\n",
"|-----------|------|---------------|-------------|\n",
"| lambdaInitial | double | 1e-5 | The initial Levenberg-Marquardt damping term |\n",
"| lambdaFactor | double | 10.0 | The amount by which to multiply or divide lambda when adjusting lambda |\n",
"| lambdaUpperBound | double | 1e5 | The maximum lambda to try before assuming the optimization has failed |\n",
"| lambdaLowerBound | double | 0.0 | The minimum lambda used in LM |\n",
"| verbosityLM | VerbosityLM | SILENT | The verbosity level for Levenberg-Marquardt |\n",
"| minModelFidelity | double | 1e-3 | Lower bound for the modelFidelity to accept the result of an LM iteration |\n",
"| logFile | std::string | \"\" | An optional CSV log file, with [iteration, time, error, lambda] |\n",
"| diagonalDamping | bool | false | If true, use diagonal of Hessian |\n",
"| useFixedLambdaFactor | bool | true | If true applies constant increase (or decrease) to lambda according to lambdaFactor |\n",
"| minDiagonal | double | 1e-6 | When using diagonal damping saturates the minimum diagonal entries |\n",
"| maxDiagonal | double | 1e32 | When using diagonal damping saturates the maximum diagonal entries |\n",
"\n",
"These parameters complement the standard optimization parameters inherited from `NonlinearOptimizerParams`, which include:\n",
"\n",
"- Maximum iterations\n",
"- Relative and absolute error thresholds\n",
"- Error function verbosity\n",
"- Linear solver type\n",
"\n",
"## Usage Notes\n",
"\n",
"- The choice of the initial guess can significantly affect the convergence speed and the quality of the solution.\n",
"- Proper tuning of the damping parameter $\\lambda$ is crucial for balancing the convergence rate and stability.\n",
"- The optimizer is most effective when the residuals are approximately linear near the solution.\n",
"\n",
"## Files\n",
"\n",
"- [LevenbergMarquardtOptimizer.h](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/LevenbergMarquardtOptimizer.h)\n",
"- [LevenbergMarquardtOptimizer.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp)\n",
"- [LevenbergMarquardtParams.h](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/LevenbergMarquardtParams.h)\n",
"- [LevenbergMarquardtParams.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/LevenbergMarquardtParams.cpp)"
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

33
gtsam/nonlinear/doc/LinearContainerFactor.ipynb Normal file
View File

@ -0,0 +1,33 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "f4c73cc1",
"metadata": {},
"source": [
"# LinearContainerFactor\n",
"\n",
"## Overview\n",
"\n",
"The `LinearContainerFactor` class in GTSAM is a specialized factor that encapsulates a linear factor within a nonlinear factor graph. This is used extensively when marginalizing out variables.\n",
"\n",
"## Key Features\n",
"\n",
"- **Encapsulation of Linear Factors**: The primary function of the `LinearContainerFactor` is to store a linear factor and its associated values, enabling it to be used within a nonlinear context.\n",
"- **Error Calculation**: It provides mechanisms to compute the error of the factor given a set of values.\n",
"- **Jacobian Computation**: The class can compute the Jacobian matrix, which is essential for optimization processes.\n",
"\n",
"## Key Methods\n",
"\n",
"- **LinearContainerFactor**: This constructor initializes the `LinearContainerFactor` with a linear factor and optionally with values. It serves as the entry point for creating an instance of this class."
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

67
gtsam/nonlinear/doc/NonlinearConjugateGradientOptimizer.ipynb Normal file
View File

@ -0,0 +1,67 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "48970ca0",
"metadata": {},
"source": [
"# NonlinearConjugateGradientOptimizer\n",
"\n",
"## Overview\n",
"\n",
"The `NonlinearConjugateGradientOptimizer` class in GTSAM is an implementation of the nonlinear conjugate gradient method for optimizing nonlinear functions. This optimizer is particularly useful for solving large-scale optimization problems where the Hessian matrix is not easily computed or stored. The conjugate gradient method is an iterative algorithm that seeks to find the minimum of a function by following a series of conjugate directions.\n",
"\n",
"The nonlinear conjugate gradient method seeks to minimize a nonlinear function $f(x)$ by iteratively updating the solution $x_k$ according to:\n",
"\n",
"$$ x_{k+1} = x_k + \\alpha_k p_k $$\n",
"\n",
"where $p_k$ is the search direction and $\\alpha_k$ is the step size determined by a line search. The search direction $p_k$ is computed using the gradient of the function and a conjugate gradient update formula, such as the Fletcher-Reeves or Polak-Ribiere formulas:\n",
"\n",
"- **Fletcher-Reeves**: \n",
" $$ \\beta_k^{FR} = \\frac{\\nabla f(x_{k+1})^T \\nabla f(x_{k+1})}{\\nabla f(x_k)^T \\nabla f(x_k)} $$\n",
" \n",
"- **Polak-Ribiere**: \n",
" $$ \\beta_k^{PR} = \\frac{\\nabla f(x_{k+1})^T (\\nabla f(x_{k+1}) - \\nabla f(x_k))}{\\nabla f(x_k)^T \\nabla f(x_k)} $$\n",
"\n",
"The choice of $\\beta_k$ affects the convergence properties of the algorithm.\n",
"\n",
"Key features:\n",
"\n",
"- **Optimization Method**: Implements the nonlinear conjugate gradient method, which is an extension of the linear conjugate gradient method to nonlinear optimization problems.\n",
"- **Efficiency**: Suitable for large-scale problems due to its iterative nature and reduced memory requirements compared to methods that require the Hessian matrix.\n",
"- **Flexibility**: Can be used with various line search strategies and conjugate gradient update formulas.\n",
"\n",
"## Key Methods\n",
"\n",
"Please see the base class [NonlinearOptimizer.ipynb](NonlinearOptimizer.ipynb).\n",
"\n",
"## Parameters\n",
"\n",
"The nonlinear conjugate gradient optimizer uses the standard optimization parameters inherited from `NonlinearOptimizerParams`, which include:\n",
"\n",
"- Maximum iterations\n",
"- Relative and absolute error thresholds\n",
"- Error function verbosity\n",
"- Linear solver type\n",
"\n",
"## Usage Notes\n",
"\n",
"- The `NonlinearConjugateGradientOptimizer` is most effective when the problem size is large and the computation of the Hessian is impractical.\n",
"- Users should choose an appropriate line search method and conjugate gradient update formula based on the specific characteristics of their optimization problem.\n",
"- Monitoring the error and values during optimization can provide insights into the convergence behavior and help diagnose potential issues.\n",
"\n",
"## Files\n",
"\n",
"- [NonlinearConjugateGradientOptimizer.h](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h)\n",
"- [NonlinearConjugateGradientOptimizer.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp)"
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

124
gtsam/nonlinear/doc/NonlinearEquality.ipynb Normal file
View File

@ -0,0 +1,124 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# NonlinearEquality\n",
"\n",
"The `NonlinearEquality` family of factors in GTSAM provides constraints to enforce equality between variables or between a variable and a constant value. These factors are useful in optimization problems where strict equality constraints are required. Below is an overview of the API, grouped by functionality."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## NonlinearEquality\n",
"\n",
"The `NonlinearEquality` factor enforces equality between a variable and a feasible value. It supports both exact and inexact evaluation modes.\n",
"\n",
"### Constructors\n",
"- `NonlinearEquality(Key j, const T& feasible, const CompareFunction& compare)` \n",
" Creates a factor that enforces exact equality between the variable at key `j` and the feasible value `feasible`. \n",
" - `j`: Key of the variable to constrain. \n",
" - `feasible`: The feasible value to enforce equality with. \n",
" - `compare`: Optional comparison function (default uses `traits<T>::Equals`).\n",
"\n",
"- `NonlinearEquality(Key j, const T& feasible, double error_gain, const CompareFunction& compare)` \n",
" Creates a factor that allows inexact evaluation with a specified error gain. \n",
" - `error_gain`: Gain applied to the error when the constraint is violated.\n",
"\n",
"### Methods\n",
"- `double error(const Values& c) const` \n",
" Computes the error for the given values. Returns `0.0` if the constraint is satisfied, or a scaled error if `allow_error_` is enabled.\n",
"\n",
"- `Vector evaluateError(const T& xj, OptionalMatrixType H = nullptr) const` \n",
" Evaluates the error vector for the given variable value `xj`. Optionally computes the Jacobian matrix `H`.\n",
"\n",
"- `GaussianFactor::shared_ptr linearize(const Values& x) const` \n",
" Linearizes the factor at the given values `x` to create a Gaussian factor."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## NonlinearEquality1\n",
"\n",
"The `NonlinearEquality1` factor is a unary equality constraint that fixes a variable to a specific value.\n",
"\n",
"### Constructors\n",
"- `NonlinearEquality1(const X& value, Key key, double mu = 1000.0)` \n",
" Creates a factor that fixes the variable at `key` to the value `value`. \n",
" - `value`: The fixed value for the variable. \n",
" - `key`: Key of the variable to constrain. \n",
" - `mu`: Strength of the constraint (default: `1000.0`).\n",
"\n",
"### Methods\n",
"- `Vector evaluateError(const X& x1, OptionalMatrixType H = nullptr) const` \n",
" Evaluates the error vector for the given variable value `x1`. Optionally computes the Jacobian matrix `H`.\n",
"\n",
"- `void print(const std::string& s, const KeyFormatter& keyFormatter) const` \n",
" Prints the factor details, including the fixed value and noise model."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## NonlinearEquality2\n",
"\n",
"The `NonlinearEquality2` factor is a binary equality constraint that enforces equality between two variables.\n",
"\n",
"### Constructors\n",
"- `NonlinearEquality2(Key key1, Key key2, double mu = 1e4)` \n",
" Creates a factor that enforces equality between the variables at `key1` and `key2`. \n",
" - `key1`: Key of the first variable. \n",
" - `key2`: Key of the second variable. \n",
" - `mu`: Strength of the constraint (default: `1e4`).\n",
"\n",
"### Methods\n",
"- `Vector evaluateError(const T& x1, const T& x2, OptionalMatrixType H1 = nullptr, OptionalMatrixType H2 = nullptr) const` \n",
" Evaluates the error vector for the given variable values `x1` and `x2`. Optionally computes the Jacobian matrices `H1` and `H2`.\n",
"\n",
"- `void print(const std::string& s, const KeyFormatter& keyFormatter) const` \n",
" Prints the factor details, including the keys and noise model."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Common Features\n",
"\n",
"### Error Handling Modes\n",
"- Exact Evaluation: Throws an error during linearization if the constraint is violated. \n",
"- Inexact Evaluation: Allows nonzero error and scales it using the `error_gain_` parameter.\n",
"\n",
"### Serialization\n",
"All factors support serialization for saving and loading models.\n",
"\n",
"### Testable Interface\n",
"All factors implement the `Testable` interface, providing methods like:\n",
"- `void print(const std::string& s, const KeyFormatter& keyFormatter) const` \n",
" Prints the factor details.\n",
"- `bool equals(const NonlinearFactor& f, double tol) const` \n",
" Checks if two factors are equal within a specified tolerance."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"These factors provide a flexible way to enforce equality constraints in nonlinear optimization problems, making them useful for applications like SLAM, robotics, and control systems."
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 2
}

84
gtsam/nonlinear/doc/NonlinearFactor.ipynb Normal file
View File

@ -0,0 +1,84 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "381ccaaa",
"metadata": {},
"source": [
"# NonlinearFactor\n",
"\n",
"## Overview\n",
"\n",
"The `NonlinearFactor` class in GTSAM is a fundamental component used in nonlinear optimization. It represents a factor in a factor graph. The class is designed to work with nonlinear, continuous functions."
]
},
{
"cell_type": "markdown",
"id": "94ffa16d",
"metadata": {},
"source": [
"## Mathematical Formulation\n",
"\n",
"The `NonlinearFactor` is generally represented by a function $f(x)$, where $x$ is a vector of variables. The error is given by:\n",
"$$\n",
"e(x) = f(x)- z\n",
"$$\n",
"where $z$ is the observed measurement. The optimization process aims to minimize the sum of squared errors:\n",
"$$\n",
"\\min_x \\sum_i \\| e_i(x) \\|^2 \n",
"$$\n",
"\n",
"Linearization involves approximating $f(x)$ around a point $x_0$:\n",
"$$\n",
"f(x) \\approx f(x_0) + A\\delta x\n",
"$$\n",
"where $A$ is the Jacobian matrix of $f$ at $x_0$, and $\\delta x \\doteq x - x_0$. This leads to a linearized error:\n",
"$$\n",
"e(x) \\approx (f(x_0) + A\\delta x) - z = A\\delta x - b\n",
"$$\n",
"where $b\\doteq z - f(x_0)$ is the prediction error."
]
},
{
"cell_type": "markdown",
"id": "e3842ba3",
"metadata": {},
"source": [
"## Key Functionalities\n",
"\n",
"### Error Calculation\n",
"\n",
"- **evaluateError**: This method computes the error vector for the factor given a set of variable values. The error is typically the difference between the predicted measurement and the actual measurement. The function can also return the Jacobian matrices if needed, which are crucial for optimization algorithms like Gauss-Newton or Levenberg-Marquardt.\n",
"\n",
"### Jacobian and Hessian\n",
"\n",
"- **linearize**: This method linearizes the nonlinear factor around a linearization point. It returns a `GaussianFactor`, which is an approximation of the `NonlinearFactor` using a first-order Taylor expansion. This is a critical step in iterative optimization methods, where the problem is repeatedly linearized and solved.\n",
"\n",
"### Active Flag\n",
"\n",
"- **active**: This function checks whether the factor should be included in the optimization process. A factor might be inactive if it does not contribute to the error, which can occur in cases of conditional constraints or gating functions.\n",
"\n",
"### Dimensionality\n",
"\n",
"- **dim**: Returns the dimensionality of the factor, which corresponds to the size of the error vector. This is important for understanding the contribution of the factor to the overall optimization problem.\n",
"\n",
"### Key Management\n",
"\n",
"- **keys**: Provides access to the keys (or variable indices) involved in the factor. This is essential for understanding which variables the factor is connected to in the factor graph.\n",
"\n",
"## Usage Notes\n",
"\n",
"- The `NonlinearFactor` class is typically used in conjunction with a `NonlinearFactorGraph`, which is a collection of such factors.\n",
"- Users need to implement the `evaluateError` method in derived classes to define the specific measurement model.\n",
"- The class is designed to be flexible and extensible, allowing for custom factors to be created for specific applications."
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

63
gtsam/nonlinear/doc/NonlinearFactorGraph.ipynb Normal file
View File

@ -0,0 +1,63 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "a58d890a",
"metadata": {},
"source": [
"# NonlinearFactorGraph\n",
"\n",
"## Overview\n",
"\n",
"The `NonlinearFactorGraph` class in GTSAM is a key component for representing and solving nonlinear factor graphs. A factor graph is a bipartite graph that represents the factorization of a function, commonly used in probabilistic graphical models. In the context of GTSAM, it is used to represent the structure of optimization problems, e.g., in the domain of simultaneous localization and mapping (SLAM) or structure from motion (SfM).\n",
"\n",
"## Key Functionalities\n",
"\n",
"### Construction and Initialization\n",
"\n",
"- **Constructor**: The class provides a default constructor to initialize an empty nonlinear factor graph.\n",
"\n",
"### Factor Management\n",
"\n",
"- **add**: This method allows adding a new factor to the graph. Factors represent constraints or measurements in the optimization problem.\n",
"- **reserve**: Pre-allocates space for a specified number of factors, optimizing memory usage when the number of factors is known in advance.\n",
"\n",
"### Graph Operations\n",
"\n",
"- **resize**: Adjusts the size of the factor graph, which can be useful when dynamically modifying the graph structure.\n",
"- **remove**: Removes a factor from the graph, identified by its index.\n",
"\n",
"### Querying and Access\n",
"\n",
"- **size**: Returns the number of factors currently in the graph.\n",
"- **empty**: Checks if the graph contains any factors.\n",
"- **at**: Accesses a specific factor by its index.\n",
"- **back**: Retrieves the last factor in the graph.\n",
"- **front**: Retrieves the first factor in the graph."
]
},
{
"cell_type": "markdown",
"id": "71b15f4c",
"metadata": {},
"source": [
"### Optimization and Linearization\n",
"\n",
"- **linearize**: Converts the nonlinear factor graph into a linear factor graph at a given linearization point. This is a crucial step in iterative optimization algorithms like [Gauss-Newton](./GaussNewtonOptimizer.ipynb) or [Levenberg-Marquardt](./LevenbergMarquardtOptimizer.ipynb).\n",
" \n",
" The linearization process involves computing the Jacobian matrices of the nonlinear functions, resulting in a linear approximation:\n",
" \n",
" $$ f(x) \\approx f(x_0) + A(x - x_0) $$\n",
" \n",
" where $A$ is the Jacobian matrix evaluated at the point $x_0$."
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

23
gtsam/nonlinear/doc/NonlinearISAM.ipynb Normal file
View File

@ -0,0 +1,23 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "2b6fc012",
"metadata": {},
"source": [
"# NonlinearISAM\n",
"\n",
"## Overview\n",
"\n",
"The `NonlinearISAM` class wraps the incremental factorization version of iSAM {cite:p}`http://dx.doi.org/10.1109/TRO.2008.2006706`. It is largely superseded by [iSAM2](./ISAM2.ipynb) but it is a simpler class, with less bells and whistles, that might be easier to debug. For background, also see the more recent booklet by {cite:t}`https://doi.org/10.1561/2300000043`.\n"
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

60
gtsam/nonlinear/doc/NonlinearOptimizer.ipynb Normal file
View File

@ -0,0 +1,60 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "2e4812da",
"metadata": {},
"source": [
"# NonlinearOptimizer\n",
"\n",
"## Overview\n",
"\n",
"The `NonlinearOptimizer` class in GTSAM is a the base class for (batch) nonlinear optimization solvers. It provides the basic API for optimizing nonlinear factor graphs, commonly used in robotics and computer vision applications.\n",
"\n",
"The primary purpose of the `NonlinearOptimizer` is to iteratively refine an initial estimate of a solution to minimize a nonlinear cost function. Specific optimization algorithms like Gauss-Newton, Levenberg-Marquardt, and Dogleg and implemented in derived class.\n",
"\n",
"## Mathematical Foundation\n",
"\n",
"The optimization process in `NonlinearOptimizer` is based on iterative methods that solve for the minimum of a nonlinear cost function. The general approach involves linearizing the nonlinear problem at the current estimate and solving the resulting linear system to update the estimate. This process is repeated until convergence criteria are met.\n",
"\n",
"The optimization problem can be formally defined as:\n",
"\n",
"$$\n",
"\\min_{x} \\sum_{i} \\| \\phi_i(x) \\|^2\n",
"$$\n",
"\n",
"where $x$ is the vector of variables to be optimized, and $\\phi_i(x)$ are the residuals of the factors in the graph.\n",
"\n",
"## Key Methods\n",
"\n",
"- The `optimize()` method is the core function of the `NonlinearOptimizer` class. It performs the optimization process, iteratively updating the estimate to converge to a local minimum of the cost function.\n",
"- The `error()` method computes the total error of the current estimate. This is typically the sum of squared errors for all factors in the graph. Mathematically, the error can be expressed as:\n",
" $$\n",
" E(x) = \\sum_{i} \\| \\phi_i(x) \\|^2\n",
" $$\n",
" where $\\phi_i(x)$ represents the residual error of the $i$-th factor.\n",
"- The `values()` method returns the current set of variable estimates. These estimates are updated during the optimization process.\n",
"- The `iterations()` method provides the number of iterations performed during the optimization process. This can be useful for analyzing the convergence behavior of the optimizer.\n",
"- The `params()` method returns the parameters used by the optimizer. These parameters can include settings like convergence thresholds, maximum iterations, and other algorithm-specific options.\n",
"\n",
"## Usage\n",
"\n",
"The `NonlinearOptimizer` class is typically not used directly. Instead, one of its derived classes, such as `GaussNewtonOptimizer`, `LevenbergMarquardtOptimizer`, or `DoglegOptimizer`, is used to perform specific types of optimization. These derived classes implement the `optimize()` method according to their respective algorithms.\n",
"\n",
"## Files\n",
"\n",
"- [NonlinearOptimizer.h](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/NonlinearOptimizer.h)\n",
"- [NonlinearOptimizer.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/NonlinearOptimizer.cpp)\n",
"- [NonlinearOptimizerParams.h](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/NonlinearOptimizerParams.h)\n",
"- [NonlinearOptimizerParams.cpp](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/NonlinearOptimizerParams.cpp)"
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

53
gtsam/nonlinear/doc/PriorFactor.ipynb Normal file
View File

@ -0,0 +1,53 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "ec35011c",
"metadata": {},
"source": [
"# PriorFactor\n",
"\n",
"## Overview\n",
"\n",
"The `PriorFactor` represents a prior belief about a variable in the form of a Gaussian distribution. This class is crucial for incorporating prior knowledge into the optimization process, which can significantly enhance the accuracy and robustness of the solutions.\n",
"\n",
"## Key Functionalities\n",
"\n",
"### PriorFactor Construction\n",
"\n",
"The `PriorFactor` is constructed by specifying a key, a prior value, and a noise model. The key identifies the variable in the factor graph, the prior value represents the expected value of the variable, and the noise model encapsulates the uncertainty associated with this prior belief.\n",
"\n",
"### Error Calculation\n",
"\n",
"The primary role of the `PriorFactor` is to compute the error between the estimated value of a variable and its prior. This error is typically defined as:\n",
"\n",
"$$\n",
"e(x) = x - \\mu\n",
"$$\n",
"\n",
"where $x$ is the estimated value, and $\\mu$ is the prior mean. The error is then weighted by the noise model to form the contribution of this factor to the overall objective function.\n",
"\n",
"### Adding to a Factor Graph\n",
"\n",
"[NonlinearFactorGraph](./NonlinearFactorGraph.ipynb) has a templated method `addPrior<T>` that provides a convenient way to add priors.\n",
"\n",
"## Usage Considerations\n",
"\n",
"- **Noise Model**: The choice of noise model is critical as it determines how strongly the prior is enforced. A tighter noise model implies a stronger belief in the prior. *Note that very strong priors might make the condition number of the linear systems to be solved very high. In this case consider adding a [NonlinearEqualityFactor]\n",
"- **Integration with Other Factors**: The `PriorFactor` is typically used in conjunction with other factors that model the system dynamics and measurements. It helps anchor the solution, especially in scenarios with limited or noisy measurements.\n",
"- **Applications**: Common applications include SLAM (Simultaneous Localization and Mapping), where priors on initial poses or landmarks can significantly improve map accuracy and convergence speed.\n",
"\n",
"## Conclusion\n",
"\n",
"The `PriorFactor` class is a fundamental component in GTSAM for incorporating prior information into the factor graph framework. By understanding its construction, error computation, and integration into the optimization process, users can effectively leverage prior knowledge to enhance their estimation and optimization tasks."
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

67
gtsam/nonlinear/doc/WhiteNoiseFactor.ipynb Normal file
View File

@ -0,0 +1,67 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "5a0c879e",
"metadata": {},
"source": [
"# WhiteNoiseFactor\n",
"\n",
"*Below is partly generated with ChatGPT 4o, needs to be verified.*\n",
"\n",
"## Overview\n",
"\n",
"The `WhiteNoiseFactor` in GTSAM is a binary nonlinear factor designed to estimate the parameters of zero-mean Gaussian white noise. It uses a **mean-precision parameterization**, where the mean $ \\mu $ and precision $ \\tau = 1/\\sigma^2 $ are treated as variables to be optimized."
]
},
{
"cell_type": "markdown",
"id": "b40b3242",
"metadata": {},
"source": [
"## Parameterization\n",
"\n",
"The factor models the negative log-likelihood of a zero-mean Gaussian distribution as follows,\n",
"$$\n",
"f(z, \\mu, \\tau) = \\log(\\sqrt{2\\pi}) - 0.5 \\log(\\tau) + 0.5 \\tau (z - \\mu)^2\n",
"$$\n",
"where:\n",
"- $ z $: Measurement value (observed data).\n",
"- $ \\mu $: Mean of the Gaussian distribution (to be estimated).\n",
"- $ \\tau $: Precision of the Gaussian distribution $ \\tau = 1/\\sigma^2 $, also to be estimated).\n",
"\n",
"This formulation allows the factor to optimize both the mean and precision of the noise model simultaneously."
]
},
{
"cell_type": "markdown",
"id": "2f36abdb",
"metadata": {},
"source": [
"## Use Case: Estimating IMU Noise Characteristics\n",
"\n",
"The `WhiteNoiseFactor` can be used in system identification tasks, such as estimating the noise characteristics of an IMU. Here's how it would work:\n",
"\n",
"1. **Define the Measurement**:\n",
" - Collect a series of IMU measurements (e.g., accelerometer or gyroscope readings) under known conditions (e.g., stationary or constant velocity).\n",
"\n",
"2. **Set Up the Factor Graph**:\n",
" - Add `WhiteNoiseFactor` instances to the factor graph for each measurement, linking the observed value $ z $ to the mean and precision variables.\n",
"\n",
"3. **Optimize the Graph**:\n",
" - Use GTSAM's nonlinear optimization tools to solve for the mean $ \\mu $ and precision $ \\tau $ that best explain the observed measurements.\n",
"\n",
"4. **Extract Noise Characteristics**:\n",
" - The optimized mean $ \\mu $ represents the bias in the sensor measurements.\n",
" - The optimized precision $ \\tau $ can be inverted to compute the standard deviation $ \\sigma = 1/\\sqrt{\\tau} $, which represents the noise level."
]
}
],
"metadata": {
"language_info": {
"name": "python"
}
},
"nbformat": 4,
"nbformat_minor": 5
}

53
gtsam/nonlinear/nonlinear.md Normal file
View File

@ -0,0 +1,53 @@
# Nonlinear
The `nonlinear` module in GTSAM includes a comprehensive set of tools for nonlinear optimization using factor graphs. Here's an overview of key components organized by category:
## Core Classes
- [NonlinearFactorGraph](doc/NonlinearFactorGraph.ipynb): Represents the optimization problem as a graph of factors.
- [NonlinearFactor](doc/NonlinearFactor.ipynb): Base class for all nonlinear factors.
- [NoiseModelFactor](doc/NonlinearFactor.ipynb): Base class for factors with noise models.
- [Values](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/Values.h): Container for variable assignments used in optimization.
## Batch Optimizers
- [NonlinearOptimizer](doc/NonlinearOptimizer.ipynb): Base class for all batch optimizers.
- [NonlinearOptimizerParams](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/NonlinearOptimizerParams.h): Base parameters class for all optimizers.
- [GaussNewtonOptimizer](doc/GaussNewtonOptimizer.ipynb): Implements Gauss-Newton optimization.
- [GaussNewtonParams](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/GaussNewtonParams.h): Parameters for Gauss-Newton optimization.
- [LevenbergMarquardtOptimizer](doc/LevenbergMarquardtOptimizer.ipynb): Implements Levenberg-Marquardt optimization.
- [LevenbergMarquardtParams](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/LevenbergMarquardtParams.h): Parameters for Levenberg-Marquardt optimization.
- [DoglegOptimizer](doc/DoglegOptimizer.ipynb): Implements Powell's Dogleg optimization.
- [DoglegParams](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/DoglegParams.h): Parameters for Dogleg optimization.
- [GncOptimizer](doc/GncOptimizer.ipynb): Implements robust optimization using Graduated Non-Convexity.
- [GncParams](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/GncParams.h): Parameters for Graduated Non-Convexity optimization.
## Incremental Optimizers
- [ISAM2](doc/ISAM2.ipynb): Incremental Smoothing and Mapping 2, with fluid relinearization.
- [ISAM2Params](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/ISAM2Params.h): Parameters controlling the ISAM2 algorithm.
- [ISAM2Result](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/ISAM2Result.h): Results from ISAM2 update operations.
- [NonlinearISAM](doc/NonlinearISAM.ipynb): Original iSAM implementation (mostly superseded by ISAM2).
## Specialized Factors
- [PriorFactor](doc/PriorFactor.ipynb): Imposes a prior constraint on a variable.
- [NonlinearEquality](doc/NonlinearEquality.ipynb): Enforces equality constraints between variables.
- [LinearContainerFactor](doc/LinearContainerFactor.ipynb): Wraps linear factors for inclusion in nonlinear factor graphs.
- [WhiteNoiseFactor](doc/WhiteNoiseFactor.ipynb): Binary factor to estimate parameters of zero-mean Gaussian white noise.
## Filtering and Smoothing
- [ExtendedKalmanFilter](doc/ExtendedKalmanFilter.ipynb): Nonlinear Kalman filter implementation.
- [FixedLagSmoother](doc/FixedLagSmoother.ipynb): Base class for fixed-lag smoothers.
- [BatchFixedLagSmoother](doc/BatchFixedLagSmoother.ipynb): Implementation of a fixed-lag smoother using batch optimization.
- [IncrementalFixedLagSmoother](doc/IncrementalFixedLagSmoother.ipynb): Implementation of a fixed-lag smoother using iSAM2.
## Analysis and Visualization
- [Marginals](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/Marginals.h): Computes marginal covariances from optimization results.
- [GraphvizFormatting](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/GraphvizFormatting.h): Provides customization for factor graph visualization.

9
myst.yml
View File

@ -8,14 +8,21 @@ project:
toc:
- file: README.md
- file: INSTALL.md
- file: ./gtsam/user_guide.md
- file: ./doc/user_guide.md
children:
- file: ./gtsam/geometry/geometry.md
children:
- pattern: ./gtsam/geometry/doc/*
- file: ./gtsam/nonlinear/nonlinear.md
children:
- pattern: ./gtsam/nonlinear/doc/*
- file: ./gtsam/navigation/navigation.md
children:
- pattern: ./gtsam/navigation/doc/*
- file: ./doc/examples.md
children:
- pattern: ./python/gtsam/examples/*.ipynb
- file: ./doc/expressions.md
site:
nav:
- title: Getting started

112
python/CustomFactors.md
View File

@ -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(<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.

188
python/gtsam/examples/CameraResectioning.ipynb Normal file
View File

@ -0,0 +1,188 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Camera Resectioning Example\n",
"\n",
"This is a 1:1 transcription of CameraResectioning.cpp, but using custom factors."
]
},
{
"cell_type": "markdown",
"metadata": {
"tags": [
"remove-cell"
]
},
"source": [
"GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n",
"Atlanta, Georgia 30332-0415\n",
"All Rights Reserved\n",
"\n",
"Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
"\n",
"See LICENSE for the license information"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/examples/CameraResectioning.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,
"metadata": {
"tags": [
"remove-cell"
],
"vscode": {
"languageId": "markdown"
}
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"from gtsam import Cal3_S2, CustomFactor, LevenbergMarquardtOptimizer, KeyVector\n",
"from gtsam import NonlinearFactor, NonlinearFactorGraph\n",
"from gtsam import PinholeCameraCal3_S2, Point2, Point3, Pose3, Rot3, Values\n",
"from gtsam.noiseModel import Base as SharedNoiseModel, Diagonal\n",
"from gtsam.symbol_shorthand import X"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"def resectioning_factor(\n",
" model: SharedNoiseModel,\n",
" key: int,\n",
" calib: Cal3_S2,\n",
" p: Point2,\n",
" P: Point3,\n",
") -> NonlinearFactor:\n",
"\n",
" def error_func(this: CustomFactor, v: Values, H: list[np.ndarray]) -> np.ndarray:\n",
" pose = v.atPose3(this.keys()[0])\n",
" camera = PinholeCameraCal3_S2(pose, calib)\n",
" if H is None:\n",
" return camera.project(P) - p\n",
" Dpose = np.zeros((2, 6), order=\"F\")\n",
" result = camera.project(P, Dpose) - p\n",
" H[0] = Dpose\n",
" return result\n",
"\n",
" return CustomFactor(model, KeyVector([key]), error_func)\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Assumptions:\n",
"- Camera: $f = 1$, Image: $100\\times100$, center: $50, 50.0$\n",
"- Pose (ground truth): $(X_w, -Y_w, -Z_w, [0,0,2.0]^T)$\n",
"- Known landmarks: $(10,10,0), (-10,10,0), (-10,-10,0), (10,-10,0)$\n",
"- Perfect measurements: $(55,45), (45,45), (45,55), (55,55)$\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Final result:\n",
"\n",
"Values with 1 values:\n",
"Value x1: (gtsam::Pose3)\n",
"R: [\n",
"\t1, 0, 0;\n",
"\t0, -1, 0;\n",
"\t0, 0, -1\n",
"]\n",
"t: 0 0 2\n",
"\n"
]
}
],
"source": [
"# Create camera intrinsic parameters\n",
"calibration = Cal3_S2(1, 1, 0, 50, 50)\n",
"\n",
"# 1. create graph\n",
"graph = NonlinearFactorGraph()\n",
"\n",
"# 2. add factors to the graph\n",
"measurement_noise = Diagonal.Sigmas(np.array([0.5, 0.5]))\n",
"graph.add(\n",
" resectioning_factor(\n",
" measurement_noise, X(1), calibration, Point2(55, 45), Point3(10, 10, 0)\n",
" )\n",
")\n",
"graph.add(\n",
" resectioning_factor(\n",
" measurement_noise, X(1), calibration, Point2(45, 45), Point3(-10, 10, 0)\n",
" )\n",
")\n",
"graph.add(\n",
" resectioning_factor(\n",
" measurement_noise, X(1), calibration, Point2(45, 55), Point3(-10, -10, 0)\n",
" )\n",
")\n",
"graph.add(\n",
" resectioning_factor(\n",
" measurement_noise, X(1), calibration, Point2(55, 55), Point3(10, -10, 0)\n",
" )\n",
")\n",
"\n",
"# 3. Create an initial estimate for the camera pose\n",
"initial: Values = Values()\n",
"initial.insert(X(1), Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 1)))\n",
"\n",
"# 4. Optimize the graph using Levenberg-Marquardt\n",
"result: Values = LevenbergMarquardtOptimizer(graph, initial).optimize()\n",
"result.print(\"Final result:\\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": 2
}

85
python/gtsam/examples/CameraResectioning.py
View File

@ -1,85 +0,0 @@
# pylint: disable=consider-using-from-import,invalid-name,no-name-in-module,no-member,missing-function-docstring
"""
This is a 1:1 transcription of CameraResectioning.cpp.
"""
import numpy as np
from gtsam import Cal3_S2, CustomFactor, LevenbergMarquardtOptimizer, KeyVector
from gtsam import NonlinearFactor, NonlinearFactorGraph
from gtsam import PinholeCameraCal3_S2, Point2, Point3, Pose3, Rot3, Values
from gtsam.noiseModel import Base as SharedNoiseModel, Diagonal
from gtsam.symbol_shorthand import X
def resectioning_factor(
model: SharedNoiseModel,
key: int,
calib: Cal3_S2,
p: Point2,
P: Point3,
) -> NonlinearFactor:
def error_func(this: CustomFactor, v: Values, H: list[np.ndarray]) -> np.ndarray:
pose = v.atPose3(this.keys()[0])
camera = PinholeCameraCal3_S2(pose, calib)
if H is None:
return camera.project(P) - p
Dpose = np.zeros((2, 6), order="F")
Dpoint = np.zeros((2, 3), order="F")
Dcal = np.zeros((2, 5), order="F")
result = camera.project(P, Dpose, Dpoint, Dcal) - p
H[0] = Dpose
return result
return CustomFactor(model, KeyVector([key]), error_func)
def main() -> None:
"""
Camera: f = 1, Image: 100x100, center: 50, 50.0
Pose (ground truth): (Xw, -Yw, -Zw, [0,0,2.0]')
Known landmarks:
3D Points: (10,10,0) (-10,10,0) (-10,-10,0) (10,-10,0)
Perfect measurements:
2D Point: (55,45) (45,45) (45,55) (55,55)
"""
# read camera intrinsic parameters
calib = Cal3_S2(1, 1, 0, 50, 50)
# 1. create graph
graph = NonlinearFactorGraph()
# 2. add factors to the graph
measurement_noise = Diagonal.Sigmas(np.array([0.5, 0.5]))
graph.add(
resectioning_factor(
measurement_noise, X(1), calib, Point2(55, 45), Point3(10, 10, 0)
)
)
graph.add(
resectioning_factor(
measurement_noise, X(1), calib, Point2(45, 45), Point3(-10, 10, 0)
)
)
graph.add(
resectioning_factor(
measurement_noise, X(1), calib, Point2(45, 55), Point3(-10, -10, 0)
)
)
graph.add(
resectioning_factor(
measurement_noise, X(1), calib, Point2(55, 55), Point3(10, -10, 0)
)
)
# 3. Create an initial estimate for the camera pose
initial: Values = Values()
initial.insert(X(1), Pose3(Rot3(1, 0, 0, 0, -1, 0, 0, 0, -1), Point3(0, 0, 1)))
# 4. Optimize the graph using Levenberg-Marquardt
result: Values = LevenbergMarquardtOptimizer(graph, initial).optimize()
result.print("Final result:\n")
if __name__ == "__main__":
main()

1013
python/gtsam/examples/DiscreteBayesTree.ipynb Normal file
View File

File diff suppressed because it is too large Load Diff

805
python/gtsam/examples/DiscreteSwitching.ipynb Normal file
View File

@ -0,0 +1,805 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# A Discrete Switching System\n",
"\n",
"A la multi-hypothesis-smoother (MHS), but all discrete.\n"
]
},
{
"cell_type": "markdown",
"metadata": {
"tags": [
"remove-cell"
]
},
"source": [
"GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n",
"Atlanta, Georgia 30332-0415\n",
"All Rights Reserved\n",
"\n",
"Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
"\n",
"See LICENSE for the license information"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/DiscreteSwitching.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,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"# This needs gtbook:\n",
"% pip install --quiet gtbook"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"from gtsam import DiscreteBayesNet, DiscreteKeys, DiscreteFactorGraph, Ordering\n",
"from gtsam.symbol_shorthand import S\n",
"from gtsam.symbol_shorthand import M"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"def P(*args):\n",
" \"\"\" Create a DiscreteKeys instances from a variable number of DiscreteKey pairs.\"\"\"\n",
" # TODO: We can make life easier by providing variable argument functions in C++ itself.\n",
" dks = DiscreteKeys()\n",
" for key in args:\n",
" dks.push_back(key)\n",
" return dks\n"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"import graphviz\n",
"\n",
"\n",
"class show(graphviz.Source):\n",
" \"\"\" Display an object with a dot method as a graph.\"\"\"\n",
"\n",
" def __init__(self, obj):\n",
" \"\"\"Construct from object with 'dot' method.\"\"\"\n",
" # This small class takes an object, calls its dot function, and uses the\n",
" # resulting string to initialize a graphviz.Source instance. This in turn\n",
" # has a _repr_mimebundle_ method, which then renders it in the notebook.\n",
" super().__init__(obj.dot())\n"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"data": {
"text/html": [
"<div><p><tt>DiscreteBayesNet</tt> of size 4</p><div>\n",
"<p> <i>P(s2|m1,s1):</i></p>\n",
"<table class='DiscreteConditional'>\n",
" <thead>\n",
" <tr><th><i>m1</i></th><th><i>s1</i></th><th>0</th><th>1</th><th>2</th></tr>\n",
" </thead>\n",
" <tbody>\n",
" <tr><th>0</th><th>0</th><td>0.9</td><td>0.1</td><td>0</td></tr>\n",
" <tr><th>0</th><th>1</th><td>0.1</td><td>0.8</td><td>0.1</td></tr>\n",
" <tr><th>0</th><th>2</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
" <tr><th>1</th><th>0</th><td>0.1</td><td>0.9</td><td>0</td></tr>\n",
" <tr><th>1</th><th>1</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
" <tr><th>1</th><th>2</th><td>0.9</td><td>0</td><td>0.1</td></tr>\n",
" </tbody>\n",
"</table>\n",
"</div>\n",
"<div>\n",
"<p> <i>P(s3|m2,s2):</i></p>\n",
"<table class='DiscreteConditional'>\n",
" <thead>\n",
" <tr><th><i>m2</i></th><th><i>s2</i></th><th>0</th><th>1</th><th>2</th></tr>\n",
" </thead>\n",
" <tbody>\n",
" <tr><th>0</th><th>0</th><td>0.9</td><td>0.1</td><td>0</td></tr>\n",
" <tr><th>0</th><th>1</th><td>0.1</td><td>0.8</td><td>0.1</td></tr>\n",
" <tr><th>0</th><th>2</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
" <tr><th>1</th><th>0</th><td>0.1</td><td>0.9</td><td>0</td></tr>\n",
" <tr><th>1</th><th>1</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
" <tr><th>1</th><th>2</th><td>0.9</td><td>0</td><td>0.1</td></tr>\n",
" </tbody>\n",
"</table>\n",
"</div>\n",
"<div>\n",
"<p> <i>P(s4|m3,s3):</i></p>\n",
"<table class='DiscreteConditional'>\n",
" <thead>\n",
" <tr><th><i>m3</i></th><th><i>s3</i></th><th>0</th><th>1</th><th>2</th></tr>\n",
" </thead>\n",
" <tbody>\n",
" <tr><th>0</th><th>0</th><td>0.9</td><td>0.1</td><td>0</td></tr>\n",
" <tr><th>0</th><th>1</th><td>0.1</td><td>0.8</td><td>0.1</td></tr>\n",
" <tr><th>0</th><th>2</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
" <tr><th>1</th><th>0</th><td>0.1</td><td>0.9</td><td>0</td></tr>\n",
" <tr><th>1</th><th>1</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
" <tr><th>1</th><th>2</th><td>0.9</td><td>0</td><td>0.1</td></tr>\n",
" </tbody>\n",
"</table>\n",
"</div>\n",
"<div>\n",
"<p> <i>P(s5|m4,s4):</i></p>\n",
"<table class='DiscreteConditional'>\n",
" <thead>\n",
" <tr><th><i>m4</i></th><th><i>s4</i></th><th>0</th><th>1</th><th>2</th></tr>\n",
" </thead>\n",
" <tbody>\n",
" <tr><th>0</th><th>0</th><td>0.9</td><td>0.1</td><td>0</td></tr>\n",
" <tr><th>0</th><th>1</th><td>0.1</td><td>0.8</td><td>0.1</td></tr>\n",
" <tr><th>0</th><th>2</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
" <tr><th>1</th><th>0</th><td>0.1</td><td>0.9</td><td>0</td></tr>\n",
" <tr><th>1</th><th>1</th><td>0</td><td>0.1</td><td>0.9</td></tr>\n",
" <tr><th>1</th><th>2</th><td>0.9</td><td>0</td><td>0.1</td></tr>\n",
" </tbody>\n",
"</table>\n",
"</div>\n"
],
"text/markdown": [
"`DiscreteBayesNet` of size 4\n",
"\n",
" *P(s2|m1,s1):*\n",
"\n",
"|*m1*|*s1*|0|1|2|\n",
"|:-:|:-:|:-:|:-:|:-:|\n",
"|0|0|0.9|0.1|0|\n",
"|0|1|0.1|0.8|0.1|\n",
"|0|2|0|0.1|0.9|\n",
"|1|0|0.1|0.9|0|\n",
"|1|1|0|0.1|0.9|\n",
"|1|2|0.9|0|0.1|\n",
"\n",
" *P(s3|m2,s2):*\n",
"\n",
"|*m2*|*s2*|0|1|2|\n",
"|:-:|:-:|:-:|:-:|:-:|\n",
"|0|0|0.9|0.1|0|\n",
"|0|1|0.1|0.8|0.1|\n",
"|0|2|0|0.1|0.9|\n",
"|1|0|0.1|0.9|0|\n",
"|1|1|0|0.1|0.9|\n",
"|1|2|0.9|0|0.1|\n",
"\n",
" *P(s4|m3,s3):*\n",
"\n",
"|*m3*|*s3*|0|1|2|\n",
"|:-:|:-:|:-:|:-:|:-:|\n",
"|0|0|0.9|0.1|0|\n",
"|0|1|0.1|0.8|0.1|\n",
"|0|2|0|0.1|0.9|\n",
"|1|0|0.1|0.9|0|\n",
"|1|1|0|0.1|0.9|\n",
"|1|2|0.9|0|0.1|\n",
"\n",
" *P(s5|m4,s4):*\n",
"\n",
"|*m4*|*s4*|0|1|2|\n",
"|:-:|:-:|:-:|:-:|:-:|\n",
"|0|0|0.9|0.1|0|\n",
"|0|1|0.1|0.8|0.1|\n",
"|0|2|0|0.1|0.9|\n",
"|1|0|0.1|0.9|0|\n",
"|1|1|0|0.1|0.9|\n",
"|1|2|0.9|0|0.1|\n",
"\n"
],
"text/plain": [
"DiscreteBayesNet\n",
" \n",
"size: 4\n",
"conditional 0: P( s2 | m1 s1 ):\n",
" Choice(s2) \n",
" 0 Choice(s1) \n",
" 0 0 Choice(m1) \n",
" 0 0 0 Leaf 0.9\n",
" 0 0 1 Leaf 0.1\n",
" 0 1 Choice(m1) \n",
" 0 1 0 Leaf 0.1\n",
" 0 1 1 Leaf 0\n",
" 0 2 Choice(m1) \n",
" 0 2 0 Leaf 0\n",
" 0 2 1 Leaf 0.9\n",
" 1 Choice(s1) \n",
" 1 0 Choice(m1) \n",
" 1 0 0 Leaf 0.1\n",
" 1 0 1 Leaf 0.9\n",
" 1 1 Choice(m1) \n",
" 1 1 0 Leaf 0.8\n",
" 1 1 1 Leaf 0.1\n",
" 1 2 Choice(m1) \n",
" 1 2 0 Leaf 0.1\n",
" 1 2 1 Leaf 0\n",
" 2 Choice(s1) \n",
" 2 0 Choice(m1) \n",
" 2 0 0 Leaf 0\n",
" 2 0 1 Leaf 0\n",
" 2 1 Choice(m1) \n",
" 2 1 0 Leaf 0.1\n",
" 2 1 1 Leaf 0.9\n",
" 2 2 Choice(m1) \n",
" 2 2 0 Leaf 0.9\n",
" 2 2 1 Leaf 0.1\n",
"\n",
"conditional 1: P( s3 | m2 s2 ):\n",
" Choice(s3) \n",
" 0 Choice(s2) \n",
" 0 0 Choice(m2) \n",
" 0 0 0 Leaf 0.9\n",
" 0 0 1 Leaf 0.1\n",
" 0 1 Choice(m2) \n",
" 0 1 0 Leaf 0.1\n",
" 0 1 1 Leaf 0\n",
" 0 2 Choice(m2) \n",
" 0 2 0 Leaf 0\n",
" 0 2 1 Leaf 0.9\n",
" 1 Choice(s2) \n",
" 1 0 Choice(m2) \n",
" 1 0 0 Leaf 0.1\n",
" 1 0 1 Leaf 0.9\n",
" 1 1 Choice(m2) \n",
" 1 1 0 Leaf 0.8\n",
" 1 1 1 Leaf 0.1\n",
" 1 2 Choice(m2) \n",
" 1 2 0 Leaf 0.1\n",
" 1 2 1 Leaf 0\n",
" 2 Choice(s2) \n",
" 2 0 Choice(m2) \n",
" 2 0 0 Leaf 0\n",
" 2 0 1 Leaf 0\n",
" 2 1 Choice(m2) \n",
" 2 1 0 Leaf 0.1\n",
" 2 1 1 Leaf 0.9\n",
" 2 2 Choice(m2) \n",
" 2 2 0 Leaf 0.9\n",
" 2 2 1 Leaf 0.1\n",
"\n",
"conditional 2: P( s4 | m3 s3 ):\n",
" Choice(s4) \n",
" 0 Choice(s3) \n",
" 0 0 Choice(m3) \n",
" 0 0 0 Leaf 0.9\n",
" 0 0 1 Leaf 0.1\n",
" 0 1 Choice(m3) \n",
" 0 1 0 Leaf 0.1\n",
" 0 1 1 Leaf 0\n",
" 0 2 Choice(m3) \n",
" 0 2 0 Leaf 0\n",
" 0 2 1 Leaf 0.9\n",
" 1 Choice(s3) \n",
" 1 0 Choice(m3) \n",
" 1 0 0 Leaf 0.1\n",
" 1 0 1 Leaf 0.9\n",
" 1 1 Choice(m3) \n",
" 1 1 0 Leaf 0.8\n",
" 1 1 1 Leaf 0.1\n",
" 1 2 Choice(m3) \n",
" 1 2 0 Leaf 0.1\n",
" 1 2 1 Leaf 0\n",
" 2 Choice(s3) \n",
" 2 0 Choice(m3) \n",
" 2 0 0 Leaf 0\n",
" 2 0 1 Leaf 0\n",
" 2 1 Choice(m3) \n",
" 2 1 0 Leaf 0.1\n",
" 2 1 1 Leaf 0.9\n",
" 2 2 Choice(m3) \n",
" 2 2 0 Leaf 0.9\n",
" 2 2 1 Leaf 0.1\n",
"\n",
"conditional 3: P( s5 | m4 s4 ):\n",
" Choice(s5) \n",
" 0 Choice(s4) \n",
" 0 0 Choice(m4) \n",
" 0 0 0 Leaf 0.9\n",
" 0 0 1 Leaf 0.1\n",
" 0 1 Choice(m4) \n",
" 0 1 0 Leaf 0.1\n",
" 0 1 1 Leaf 0\n",
" 0 2 Choice(m4) \n",
" 0 2 0 Leaf 0\n",
" 0 2 1 Leaf 0.9\n",
" 1 Choice(s4) \n",
" 1 0 Choice(m4) \n",
" 1 0 0 Leaf 0.1\n",
" 1 0 1 Leaf 0.9\n",
" 1 1 Choice(m4) \n",
" 1 1 0 Leaf 0.8\n",
" 1 1 1 Leaf 0.1\n",
" 1 2 Choice(m4) \n",
" 1 2 0 Leaf 0.1\n",
" 1 2 1 Leaf 0\n",
" 2 Choice(s4) \n",
" 2 0 Choice(m4) \n",
" 2 0 0 Leaf 0\n",
" 2 0 1 Leaf 0\n",
" 2 1 Choice(m4) \n",
" 2 1 0 Leaf 0.1\n",
" 2 1 1 Leaf 0.9\n",
" 2 2 Choice(m4) \n",
" 2 2 0 Leaf 0.9\n",
" 2 2 1 Leaf 0.1\n"
]
},
"execution_count": 4,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"nrStates = 3\n",
"K = 5\n",
"\n",
"bayesNet = DiscreteBayesNet()\n",
"for k in range(1, K):\n",
" key = S(k), nrStates\n",
" key_plus = S(k+1), nrStates\n",
" mode = M(k), 2\n",
" bayesNet.add(key_plus, P(mode, key), \"9/1/0 1/8/1 0/1/9 1/9/0 0/1/9 9/0/1\")\n",
"\n",
"bayesNet"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
"<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
"<!-- Generated by graphviz version 12.0.0 (0)\n",
" -->\n",
"<!-- Pages: 1 -->\n",
"<svg width=\"242pt\" height=\"332pt\"\n",
" viewBox=\"0.00 0.00 242.00 332.00\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
"<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 328)\">\n",
"<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-328 238,-328 238,4 -4,4\"/>\n",
"<!-- var7854277750134145025 -->\n",
"<g id=\"node1\" class=\"node\">\n",
"<title>var7854277750134145025</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-306\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"27\" y=\"-300.95\" font-family=\"Times,serif\" font-size=\"14.00\">m1</text>\n",
"</g>\n",
"<!-- var8286623314361712642 -->\n",
"<g id=\"node6\" class=\"node\">\n",
"<title>var8286623314361712642</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"63\" cy=\"-234\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"63\" y=\"-228.95\" font-family=\"Times,serif\" font-size=\"14.00\">s2</text>\n",
"</g>\n",
"<!-- var7854277750134145025&#45;&gt;var8286623314361712642 -->\n",
"<g id=\"edge7\" class=\"edge\">\n",
"<title>var7854277750134145025&#45;&gt;var8286623314361712642</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M35.35,-288.76C39.58,-280.55 44.81,-270.37 49.58,-261.09\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"52.54,-262.99 54,-252.49 46.32,-259.79 52.54,-262.99\"/>\n",
"</g>\n",
"<!-- var7854277750134145026 -->\n",
"<g id=\"node2\" class=\"node\">\n",
"<title>var7854277750134145026</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"135\" cy=\"-234\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"135\" y=\"-228.95\" font-family=\"Times,serif\" font-size=\"14.00\">m2</text>\n",
"</g>\n",
"<!-- var8286623314361712643 -->\n",
"<g id=\"node7\" class=\"node\">\n",
"<title>var8286623314361712643</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-162\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"99\" y=\"-156.95\" font-family=\"Times,serif\" font-size=\"14.00\">s3</text>\n",
"</g>\n",
"<!-- var7854277750134145026&#45;&gt;var8286623314361712643 -->\n",
"<g id=\"edge5\" class=\"edge\">\n",
"<title>var7854277750134145026&#45;&gt;var8286623314361712643</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M126.65,-216.76C122.42,-208.55 117.19,-198.37 112.42,-189.09\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"115.68,-187.79 108,-180.49 109.46,-190.99 115.68,-187.79\"/>\n",
"</g>\n",
"<!-- var7854277750134145027 -->\n",
"<g id=\"node3\" class=\"node\">\n",
"<title>var7854277750134145027</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-162\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"171\" y=\"-156.95\" font-family=\"Times,serif\" font-size=\"14.00\">m3</text>\n",
"</g>\n",
"<!-- var8286623314361712644 -->\n",
"<g id=\"node8\" class=\"node\">\n",
"<title>var8286623314361712644</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"135\" cy=\"-90\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"135\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">s4</text>\n",
"</g>\n",
"<!-- var7854277750134145027&#45;&gt;var8286623314361712644 -->\n",
"<g id=\"edge3\" class=\"edge\">\n",
"<title>var7854277750134145027&#45;&gt;var8286623314361712644</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M162.65,-144.76C158.42,-136.55 153.19,-126.37 148.42,-117.09\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"151.68,-115.79 144,-108.49 145.46,-118.99 151.68,-115.79\"/>\n",
"</g>\n",
"<!-- var7854277750134145028 -->\n",
"<g id=\"node4\" class=\"node\">\n",
"<title>var7854277750134145028</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"207\" cy=\"-90\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"207\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">m4</text>\n",
"</g>\n",
"<!-- var8286623314361712645 -->\n",
"<g id=\"node9\" class=\"node\">\n",
"<title>var8286623314361712645</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-18\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"171\" y=\"-12.95\" font-family=\"Times,serif\" font-size=\"14.00\">s5</text>\n",
"</g>\n",
"<!-- var7854277750134145028&#45;&gt;var8286623314361712645 -->\n",
"<g id=\"edge1\" class=\"edge\">\n",
"<title>var7854277750134145028&#45;&gt;var8286623314361712645</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M198.65,-72.76C194.42,-64.55 189.19,-54.37 184.42,-45.09\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"187.68,-43.79 180,-36.49 181.46,-46.99 187.68,-43.79\"/>\n",
"</g>\n",
"<!-- var8286623314361712641 -->\n",
"<g id=\"node5\" class=\"node\">\n",
"<title>var8286623314361712641</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-306\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"99\" y=\"-300.95\" font-family=\"Times,serif\" font-size=\"14.00\">s1</text>\n",
"</g>\n",
"<!-- var8286623314361712641&#45;&gt;var8286623314361712642 -->\n",
"<g id=\"edge8\" class=\"edge\">\n",
"<title>var8286623314361712641&#45;&gt;var8286623314361712642</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M90.65,-288.76C86.42,-280.55 81.19,-270.37 76.42,-261.09\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"79.68,-259.79 72,-252.49 73.46,-262.99 79.68,-259.79\"/>\n",
"</g>\n",
"<!-- var8286623314361712642&#45;&gt;var8286623314361712643 -->\n",
"<g id=\"edge6\" class=\"edge\">\n",
"<title>var8286623314361712642&#45;&gt;var8286623314361712643</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M71.35,-216.76C75.58,-208.55 80.81,-198.37 85.58,-189.09\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"88.54,-190.99 90,-180.49 82.32,-187.79 88.54,-190.99\"/>\n",
"</g>\n",
"<!-- var8286623314361712643&#45;&gt;var8286623314361712644 -->\n",
"<g id=\"edge4\" class=\"edge\">\n",
"<title>var8286623314361712643&#45;&gt;var8286623314361712644</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M107.35,-144.76C111.58,-136.55 116.81,-126.37 121.58,-117.09\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"124.54,-118.99 126,-108.49 118.32,-115.79 124.54,-118.99\"/>\n",
"</g>\n",
"<!-- var8286623314361712644&#45;&gt;var8286623314361712645 -->\n",
"<g id=\"edge2\" class=\"edge\">\n",
"<title>var8286623314361712644&#45;&gt;var8286623314361712645</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M143.35,-72.76C147.58,-64.55 152.81,-54.37 157.58,-45.09\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"160.54,-46.99 162,-36.49 154.32,-43.79 160.54,-46.99\"/>\n",
"</g>\n",
"</g>\n",
"</svg>\n"
],
"text/plain": [
"<__main__.show at 0x11216aea0>"
]
},
"execution_count": 5,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"show(bayesNet)"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
"<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
"<!-- Generated by graphviz version 12.0.0 (0)\n",
" -->\n",
"<!-- Pages: 1 -->\n",
"<svg width=\"360pt\" height=\"47pt\"\n",
" viewBox=\"0.00 0.00 360.00 47.17\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
"<g id=\"graph0\" class=\"graph\" transform=\"scale(0.564263 0.564263) rotate(0) translate(4 79.6)\">\n",
"<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-79.6 634,-79.6 634,4 -4,4\"/>\n",
"<!-- var7854277750134145025 -->\n",
"<g id=\"node1\" class=\"node\">\n",
"<title>var7854277750134145025</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"27\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">m1</text>\n",
"</g>\n",
"<!-- factor0 -->\n",
"<g id=\"node10\" class=\"node\">\n",
"<title>factor0</title>\n",
"<ellipse fill=\"black\" stroke=\"black\" cx=\"99\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
"</g>\n",
"<!-- var7854277750134145025&#45;&#45;factor0 -->\n",
"<g id=\"edge2\" class=\"edge\">\n",
"<title>var7854277750134145025&#45;&#45;factor0</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M44.43,-43.58C63.74,-29.15 92.87,-7.38 98.16,-3.43\"/>\n",
"</g>\n",
"<!-- var7854277750134145026 -->\n",
"<g id=\"node2\" class=\"node\">\n",
"<title>var7854277750134145026</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"243\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"243\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">m2</text>\n",
"</g>\n",
"<!-- factor1 -->\n",
"<g id=\"node11\" class=\"node\">\n",
"<title>factor1</title>\n",
"<ellipse fill=\"black\" stroke=\"black\" cx=\"243\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
"</g>\n",
"<!-- var7854277750134145026&#45;&#45;factor1 -->\n",
"<g id=\"edge5\" class=\"edge\">\n",
"<title>var7854277750134145026&#45;&#45;factor1</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M243,-39.28C243,-25.77 243,-8.54 243,-3.96\"/>\n",
"</g>\n",
"<!-- var7854277750134145027 -->\n",
"<g id=\"node3\" class=\"node\">\n",
"<title>var7854277750134145027</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"387\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"387\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">m3</text>\n",
"</g>\n",
"<!-- factor2 -->\n",
"<g id=\"node12\" class=\"node\">\n",
"<title>factor2</title>\n",
"<ellipse fill=\"black\" stroke=\"black\" cx=\"387\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
"</g>\n",
"<!-- var7854277750134145027&#45;&#45;factor2 -->\n",
"<g id=\"edge8\" class=\"edge\">\n",
"<title>var7854277750134145027&#45;&#45;factor2</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M387,-39.28C387,-25.77 387,-8.54 387,-3.96\"/>\n",
"</g>\n",
"<!-- var7854277750134145028 -->\n",
"<g id=\"node4\" class=\"node\">\n",
"<title>var7854277750134145028</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"531\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"531\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">m4</text>\n",
"</g>\n",
"<!-- factor3 -->\n",
"<g id=\"node13\" class=\"node\">\n",
"<title>factor3</title>\n",
"<ellipse fill=\"black\" stroke=\"black\" cx=\"531\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
"</g>\n",
"<!-- var7854277750134145028&#45;&#45;factor3 -->\n",
"<g id=\"edge11\" class=\"edge\">\n",
"<title>var7854277750134145028&#45;&#45;factor3</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M531,-39.28C531,-25.77 531,-8.54 531,-3.96\"/>\n",
"</g>\n",
"<!-- var8286623314361712641 -->\n",
"<g id=\"node5\" class=\"node\">\n",
"<title>var8286623314361712641</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"99\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">s1</text>\n",
"</g>\n",
"<!-- var8286623314361712641&#45;&#45;factor0 -->\n",
"<g id=\"edge3\" class=\"edge\">\n",
"<title>var8286623314361712641&#45;&#45;factor0</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M99,-39.28C99,-25.77 99,-8.54 99,-3.96\"/>\n",
"</g>\n",
"<!-- var8286623314361712642 -->\n",
"<g id=\"node6\" class=\"node\">\n",
"<title>var8286623314361712642</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"171\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">s2</text>\n",
"</g>\n",
"<!-- var8286623314361712642&#45;&#45;factor0 -->\n",
"<g id=\"edge1\" class=\"edge\">\n",
"<title>var8286623314361712642&#45;&#45;factor0</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M153.57,-43.58C134.26,-29.15 105.13,-7.38 99.84,-3.43\"/>\n",
"</g>\n",
"<!-- var8286623314361712642&#45;&#45;factor1 -->\n",
"<g id=\"edge6\" class=\"edge\">\n",
"<title>var8286623314361712642&#45;&#45;factor1</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M188.43,-43.58C207.74,-29.15 236.87,-7.38 242.16,-3.43\"/>\n",
"</g>\n",
"<!-- var8286623314361712643 -->\n",
"<g id=\"node7\" class=\"node\">\n",
"<title>var8286623314361712643</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"315\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"315\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">s3</text>\n",
"</g>\n",
"<!-- var8286623314361712643&#45;&#45;factor1 -->\n",
"<g id=\"edge4\" class=\"edge\">\n",
"<title>var8286623314361712643&#45;&#45;factor1</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M297.57,-43.58C278.26,-29.15 249.13,-7.38 243.84,-3.43\"/>\n",
"</g>\n",
"<!-- var8286623314361712643&#45;&#45;factor2 -->\n",
"<g id=\"edge9\" class=\"edge\">\n",
"<title>var8286623314361712643&#45;&#45;factor2</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M332.43,-43.58C351.74,-29.15 380.87,-7.38 386.16,-3.43\"/>\n",
"</g>\n",
"<!-- var8286623314361712644 -->\n",
"<g id=\"node8\" class=\"node\">\n",
"<title>var8286623314361712644</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"459\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"459\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">s4</text>\n",
"</g>\n",
"<!-- var8286623314361712644&#45;&#45;factor2 -->\n",
"<g id=\"edge7\" class=\"edge\">\n",
"<title>var8286623314361712644&#45;&#45;factor2</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M441.57,-43.58C422.26,-29.15 393.13,-7.38 387.84,-3.43\"/>\n",
"</g>\n",
"<!-- var8286623314361712644&#45;&#45;factor3 -->\n",
"<g id=\"edge12\" class=\"edge\">\n",
"<title>var8286623314361712644&#45;&#45;factor3</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M476.43,-43.58C495.74,-29.15 524.87,-7.38 530.16,-3.43\"/>\n",
"</g>\n",
"<!-- var8286623314361712645 -->\n",
"<g id=\"node9\" class=\"node\">\n",
"<title>var8286623314361712645</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"603\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"603\" y=\"-52.55\" font-family=\"Times,serif\" font-size=\"14.00\">s5</text>\n",
"</g>\n",
"<!-- var8286623314361712645&#45;&#45;factor3 -->\n",
"<g id=\"edge10\" class=\"edge\">\n",
"<title>var8286623314361712645&#45;&#45;factor3</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M585.57,-43.58C566.26,-29.15 537.13,-7.38 531.84,-3.43\"/>\n",
"</g>\n",
"</g>\n",
"</svg>\n"
],
"text/plain": [
"<__main__.show at 0x1121a44d0>"
]
},
"execution_count": 6,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"# Create a factor graph out of the Bayes net.\n",
"factorGraph = DiscreteFactorGraph(bayesNet)\n",
"show(factorGraph)"
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Position 0: s1, s2, s3, s4, s5, m1, m2, m3, m4\n",
"\n"
]
}
],
"source": [
"# Create a BayesTree out of the factor graph.\n",
"ordering = Ordering()\n",
"# First eliminate \"continuous\" states in time order\n",
"for k in range(1, K+1):\n",
" ordering.push_back(S(k))\n",
"for k in range(1, K):\n",
" ordering.push_back(M(k))\n",
"print(ordering)\n",
"bayesTree = factorGraph.eliminateMultifrontal(ordering)"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [
{
"data": {
"image/svg+xml": [
"<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
"<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
"<!-- Generated by graphviz version 12.0.0 (0)\n",
" -->\n",
"<!-- Title: G Pages: 1 -->\n",
"<svg width=\"212pt\" height=\"260pt\"\n",
" viewBox=\"0.00 0.00 212.04 260.00\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
"<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 256)\">\n",
"<title>G</title>\n",
"<polygon fill=\"white\" stroke=\"none\" points=\"-4,4 -4,-256 208.04,-256 208.04,4 -4,4\"/>\n",
"<!-- 0 -->\n",
"<g id=\"node1\" class=\"node\">\n",
"<title>0</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"102.02\" cy=\"-234\" rx=\"102.02\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"102.02\" y=\"-228.95\" font-family=\"Times,serif\" font-size=\"14.00\">s4, s5, m1, m2, m3, m4</text>\n",
"</g>\n",
"<!-- 1 -->\n",
"<g id=\"node2\" class=\"node\">\n",
"<title>1</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"102.02\" cy=\"-162\" rx=\"87.18\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"102.02\" y=\"-156.95\" font-family=\"Times,serif\" font-size=\"14.00\">s3 : m1, m2, m3, s4</text>\n",
"</g>\n",
"<!-- 0&#45;&gt;1 -->\n",
"<g id=\"edge1\" class=\"edge\">\n",
"<title>0&#45;&gt;1</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M102.02,-215.7C102.02,-208.41 102.02,-199.73 102.02,-191.54\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"105.52,-191.62 102.02,-181.62 98.52,-191.62 105.52,-191.62\"/>\n",
"</g>\n",
"<!-- 2 -->\n",
"<g id=\"node3\" class=\"node\">\n",
"<title>2</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"102.02\" cy=\"-90\" rx=\"69.78\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"102.02\" y=\"-84.95\" font-family=\"Times,serif\" font-size=\"14.00\">s2 : m1, m2, s3</text>\n",
"</g>\n",
"<!-- 1&#45;&gt;2 -->\n",
"<g id=\"edge2\" class=\"edge\">\n",
"<title>1&#45;&gt;2</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M102.02,-143.7C102.02,-136.41 102.02,-127.73 102.02,-119.54\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"105.52,-119.62 102.02,-109.62 98.52,-119.62 105.52,-119.62\"/>\n",
"</g>\n",
"<!-- 3 -->\n",
"<g id=\"node4\" class=\"node\">\n",
"<title>3</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"102.02\" cy=\"-18\" rx=\"52.38\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"102.02\" y=\"-12.95\" font-family=\"Times,serif\" font-size=\"14.00\">s1 : m1, s2</text>\n",
"</g>\n",
"<!-- 2&#45;&gt;3 -->\n",
"<g id=\"edge3\" class=\"edge\">\n",
"<title>2&#45;&gt;3</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M102.02,-71.7C102.02,-64.41 102.02,-55.73 102.02,-47.54\"/>\n",
"<polygon fill=\"black\" stroke=\"black\" points=\"105.52,-47.62 102.02,-37.62 98.52,-47.62 105.52,-47.62\"/>\n",
"</g>\n",
"</g>\n",
"</svg>\n"
],
"text/plain": [
"<__main__.show at 0x11775c3e0>"
]
},
"execution_count": 8,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"show(bayesTree)"
]
}
],
"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"
},
"orig_nbformat": 4
},
"nbformat": 4,
"nbformat_minor": 2
}

43
python/gtsam/examples/EqF.ipynb
View File

@ -6,18 +6,44 @@
"source": [
"# Equivariant Filter Bias\n",
"\n",
"Implementing the example in [Fornasier et al, 2022, Overcoming Bias: Equivariant Filter Design for Biased Attitude Estimation with Online Calibration](https://arxiv.org/pdf/2209.12038).\n",
"Implementing the example in the \"Overcoming Bias\" paper by {cite:t}`https://doi.org/10.1109/LRA.2022.3210867` ([arxiv version](https://arxiv.org/pdf/2209.12038)).\n",
"This notebook was created by converting [Alessandro Fornasier's equivariant filter code](https://github.com/aau-cns/ABC-EqF) to use GTSAM's built-in data structures.\n",
"\n",
"This notebook uses Alessandro Fornasier's equivariant filter code (https://github.com/aau-cns/ABC-EqF) converted to use GTSAM's libraries.\n",
"Authors: Jennifer Oum & Darshan Rajasekaran\n",
"Authors: Jennifer Oum & Darshan Rajasekaran"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/EqF.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "markdown",
"metadata": {
"tags": [
"remove-cell"
]
},
"source": [
"GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n",
"Atlanta, Georgia 30332-0415\n",
"All Rights Reserved\n",
"\n",
"We start by installing gtsam (GTSAM's python wrapper) and gtbook."
"Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
"\n",
"See LICENSE for the license information"
]
},
{
"cell_type": "code",
"execution_count": 68,
"metadata": {},
"execution_count": null,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [
{
"name": "stdout",
@ -28,12 +54,13 @@
}
],
"source": [
"# We start by installing gtsam (GTSAM's python wrapper) and gtbook.\n",
"%pip install --quiet gtsam gtbook"
]
},
{
"cell_type": "code",
"execution_count": 69,
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
@ -47,7 +74,7 @@
"import gtsam\n",
"from gtsam import findExampleDataFile, Rot3\n",
"\n",
"from EqF import *\n"
"from EqF import *"
]
},
{

44
python/gtsam/examples/RangeISAMExample_plaza2.ipynb
View File

@ -3,6 +3,21 @@
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Range SLAM with iSAM\n",
"\n",
"A 2D Range SLAM example, with iSAM and smart range factors\n",
"\n",
"Author: Frank Dellaert"
]
},
{
"cell_type": "markdown",
"metadata": {
"tags": [
"remove-cell"
]
},
"source": [
"GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n",
"Atlanta, Georgia 30332-0415\n",
@ -10,11 +25,30 @@
"\n",
"Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
"\n",
"See LICENSE for the license information\n",
"\n",
"A 2D Range SLAM example, with iSAM and smart range factors\n",
"\n",
"Author: Frank Dellaert"
"See LICENSE for the license information"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/RangeISAMExample_plaza2.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,
"metadata": {
"tags": [
"remove-cell"
],
"vscode": {
"languageId": "markdown"
}
},
"outputs": [],
"source": [
"%pip install --quiet gtsam-develop"
]
},
{

74
python/gtsam/notebooks/easyPoint2KalmanFilter.ipynb → python/gtsam/examples/easyPoint2KalmanFilter.ipynb
View File

@ -1,24 +1,76 @@
{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"cell_type": "markdown",
"metadata": {},
"outputs": [],
"source": [
"\"\"\"\n",
"# Extended Kalman Filter\n",
"\n",
"Extended Kalman filter on a moving 2D point, but done using factor graphs.\n",
"This example uses the ExtendedKalmanFilter class to perform filtering\n",
"on a linear system, demonstrating the same operations as in elaboratePoint2KalmanFilter.\n",
"\"\"\"\n",
"\n",
"Author: Matt Kielo"
]
},
{
"cell_type": "markdown",
"metadata": {
"tags": [
"remove-cell"
]
},
"source": [
"GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n",
"Atlanta, Georgia 30332-0415\n",
"All Rights Reserved\n",
"\n",
"Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
"\n",
"See LICENSE for the license information"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/easyPoint2KalmanFilter.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,
"metadata": {
"tags": [
"remove-cell"
]
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Note: you may need to restart the kernel to use updated packages.\n"
]
}
],
"source": [
"%pip install --quiet gtsam gtbook"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np"
]
},
{
"cell_type": "code",
"execution_count": 2,
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
@ -49,7 +101,7 @@
},
{
"cell_type": "code",
"execution_count": 3,
"execution_count": 4,
"metadata": {},
"outputs": [
{
@ -64,8 +116,8 @@
"X3 Update: [3. 0.]\n",
"\n",
"Easy Final Covariance (after update):\n",
" [[0.0193 0. ]\n",
" [0. 0.0193]]\n"
" [[0.01930567 0. ]\n",
" [0. 0.01930567]]\n"
]
}
],
@ -100,7 +152,7 @@
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"display_name": "py312",
"language": "python",
"name": "python3"
},
@ -114,7 +166,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.10.12"
"version": "3.12.6"
}
},
"nbformat": 4,

53
python/gtsam/notebooks/elaboratePoint2KalmanFilter.ipynb → python/gtsam/examples/elaboratePoint2KalmanFilter.ipynb
View File

@ -1,34 +1,69 @@
{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"cell_type": "markdown",
"metadata": {},
"outputs": [],
"source": [
"\"\"\"\n",
"\n",
"# Elaborate EKF Example\n",
"\n",
"Simple linear Kalman filter on a moving 2D point using factor graphs in GTSAM.\n",
"This example manually creates all of the needed data structures to show how\n",
"the Kalman filter works under the hood using factor graphs, but uses a loop\n",
"to handle the repetitive prediction and update steps.\n",
"\n",
"Based on the C++ example by Frank Dellaert and Stephen Williams\n",
"\"\"\"\n",
"Author: Matt Kielo. Based on the C++ example by Frank Dellaert and Stephen Williams"
]
},
{
"cell_type": "markdown",
"metadata": {
"tags": [
"remove-cell"
]
},
"source": [
"GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n",
"Atlanta, Georgia 30332-0415\n",
"All Rights Reserved\n",
"\n",
"Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
"\n",
"See LICENSE for the license information"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/elaboratePoint2KalmanFilter.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,
"metadata": {},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Point2, noiseModel\n",
"from gtsam.symbol_shorthand import X"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The code below basically implements the SRIF (Square-root Information filter version of the EKF) with Cholesky factorization."
]
},
{
"cell_type": "code",
"execution_count": 2,
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# [code below basically does SRIF with Cholesky]\n",
"\n",
"# Setup containers for linearization points\n",
"linearization_points = gtsam.Values()\n",
"\n",

200
python/gtsam/notebooks/DiscreteBayesTree.ipynb
View File

File diff suppressed because one or more lines are too long

155
python/gtsam/notebooks/DiscreteSwitching.ipynb
View File

@ -1,155 +0,0 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# A Discrete Switching System\n",
"\n",
"A la MHS, but all discrete.\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"from gtsam import DiscreteBayesNet, DiscreteKeys, DiscreteFactorGraph, Ordering\n",
"from gtsam.symbol_shorthand import S\n",
"from gtsam.symbol_shorthand import M\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"def P(*args):\n",
" \"\"\" Create a DiscreteKeys instances from a variable number of DiscreteKey pairs.\"\"\"\n",
" # TODO: We can make life easier by providing variable argument functions in C++ itself.\n",
" dks = DiscreteKeys()\n",
" for key in args:\n",
" dks.push_back(key)\n",
" return dks\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"import graphviz\n",
"\n",
"\n",
"class show(graphviz.Source):\n",
" \"\"\" Display an object with a dot method as a graph.\"\"\"\n",
"\n",
" def __init__(self, obj):\n",
" \"\"\"Construct from object with 'dot' method.\"\"\"\n",
" # This small class takes an object, calls its dot function, and uses the\n",
" # resulting string to initialize a graphviz.Source instance. This in turn\n",
" # has a _repr_mimebundle_ method, which then renders it in the notebook.\n",
" super().__init__(obj.dot())\n"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"nrStates = 3\n",
"K = 5\n",
"\n",
"bayesNet = DiscreteBayesNet()\n",
"for k in range(1, K):\n",
" key = S(k), nrStates\n",
" key_plus = S(k+1), nrStates\n",
" mode = M(k), 2\n",
" bayesNet.add(key_plus, P(mode, key), \"9/1/0 1/8/1 0/1/9 1/9/0 0/1/9 9/0/1\")\n",
"\n",
"bayesNet"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"show(bayesNet)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Create a factor graph out of the Bayes net.\n",
"factorGraph = DiscreteFactorGraph(bayesNet)\n",
"show(factorGraph)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"# Create a BayesTree out of the factor graph.\n",
"ordering = Ordering()\n",
"# First eliminate \"continuous\" states in time order\n",
"for k in range(1, K+1):\n",
" ordering.push_back(S(k))\n",
"for k in range(1, K):\n",
" ordering.push_back(M(k))\n",
"print(ordering)\n",
"bayesTree = factorGraph.eliminateMultifrontal(ordering)\n",
"bayesTree"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": [
"show(bayesTree)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": []
}
],
"metadata": {
"interpreter": {
"hash": "31f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6"
},
"kernelspec": {
"display_name": "Python 3.8.9 64-bit",
"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.9.7"
},
"orig_nbformat": 4
},
"nbformat": 4,
"nbformat_minor": 2
}