Skip to content

Commit 648eafd

Browse files
dellaertdellaert
committed
Add docs
1 parent 7c691c3 commit 648eafd

File tree

2 files changed

+307
-2
lines changed

2 files changed

+307
-2
lines changed

‎gtsam/nonlinear/doc/Marginals.ipynb‎

Lines changed: 304 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,304 @@
1+
{
2+
"cells": [
3+
{
4+
"cell_type": "markdown",
5+
"id": "gtsam_nonlinear_doc_marginals__title",
6+
"metadata": {},
7+
"source": [
8+
"# Marginals\n",
9+
"\n",
10+
"## Overview\n",
11+
"\n",
12+
"The `Marginals` class in GTSAM computes Gaussian marginals from a nonlinear or linear factor graph around a chosen linearization point. In the common nonlinear case, the usual pattern is:\n",
13+
"\n",
14+
"1. build a `NonlinearFactorGraph`,\n",
15+
"2. optimize it to obtain a solution `Values`, and\n",
16+
"3. construct `Marginals(graph, result)` to query uncertainty.\n",
17+
"\n",
18+
"The most common queries are single-variable marginal covariance, single-variable marginal information, and joint covariance or information for a small set of variables. Internally, GTSAM uses the Gaussian Bayes tree to answer these queries efficiently, but users normally just call the `Marginals` methods directly.\n",
19+
"\n",
20+
"For the algorithmic story behind Bayes-tree covariance recovery, see the blog post [Fast Covariance Recovery in GTSAM Bayes Trees](https://gtsam.org/2026/03/29/bayes-tree-covariance-recovery.html). For full technical details, see [CovarianceRecovery.pdf](https://github.com/borglab/gtsam/blob/develop/doc/CovarianceRecovery.pdf)."
21+
]
22+
},
23+
{
24+
"cell_type": "markdown",
25+
"id": "gtsam_nonlinear_doc_marginals__license_cell",
26+
"metadata": {
27+
"tags": [
28+
"remove-cell"
29+
]
30+
},
31+
"source": [
32+
"GTSAM Copyright 2010-2026, Georgia Tech Research Corporation,\n",
33+
"Atlanta, Georgia 30332-0415\n",
34+
"All Rights Reserved\n",
35+
"\n",
36+
"Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
37+
"\n",
38+
"See LICENSE for the license information"
39+
]
40+
},
41+
{
42+
"cell_type": "markdown",
43+
"id": "gtsam_nonlinear_doc_marginals__colab_button",
44+
"metadata": {},
45+
"source": [
46+
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/nonlinear/doc/Marginals.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
47+
]
48+
},
49+
{
50+
"cell_type": "code",
51+
"execution_count": 1,
52+
"id": "gtsam_nonlinear_doc_marginals__colab_install",
53+
"metadata": {
54+
"tags": [
55+
"remove-cell"
56+
]
57+
},
58+
"outputs": [],
59+
"source": [
60+
"try:\n",
61+
" import google.colab\n",
62+
" %pip install --quiet gtsam-develop\n",
63+
"except ImportError:\n",
64+
" pass"
65+
]
66+
},
67+
{
68+
"cell_type": "code",
69+
"execution_count": 2,
70+
"id": "gtsam_nonlinear_doc_marginals__imports",
71+
"metadata": {},
72+
"outputs": [],
73+
"source": [
74+
"import numpy as np\n",
75+
"import gtsam\n",
76+
"from gtsam.symbol_shorthand import L, X"
77+
]
78+
},
79+
{
80+
"cell_type": "markdown",
81+
"id": "gtsam_nonlinear_doc_marginals__what_it_returns",
82+
"metadata": {},
83+
"source": [
84+
"## What `Marginals` returns\n",
85+
"\n",
86+
"The main methods are:\n",
87+
"\n",
88+
"- `marginalCovariance(key)`: dense covariance matrix for one variable\n",
89+
"- `marginalInformation(key)`: dense information matrix for one variable\n",
90+
"- `jointMarginalCovariance(keys)`: `JointMarginal` containing a joint covariance over several variables\n",
91+
"- `jointMarginalInformation(keys)`: `JointMarginal` containing a joint information matrix over several variables\n",
92+
"\n",
93+
"A `JointMarginal` lets you either access the full dense matrix with `fullMatrix()` or request specific blocks with `at(key_i, key_j)`. In practice, `at(...)` is the safest way to retrieve blocks because it avoids relying on the internal block layout."
94+
]
95+
},
96+
{
97+
"cell_type": "markdown",
98+
"id": "gtsam_nonlinear_doc_marginals__example_intro",
99+
"metadata": {},
100+
"source": [
101+
"## A small planar SLAM example\n",
102+
"\n",
103+
"The example below creates a tiny nonlinear SLAM problem with three `Pose2` variables and two `Point2` landmarks. After optimizing the graph, we use `Marginals` to recover several different uncertainty queries."
104+
]
105+
},
106+
{
107+
"cell_type": "code",
108+
"execution_count": 3,
109+
"id": "gtsam_nonlinear_doc_marginals__problem_setup",
110+
"metadata": {
111+
"tags": [
112+
"hide-input"
113+
]
114+
},
115+
"outputs": [],
116+
"source": [
117+
"def create_planar_slam_problem():\n",
118+
" graph = gtsam.NonlinearFactorGraph()\n",
119+
"\n",
120+
" x1 = X(1)\n",
121+
" x2 = X(2)\n",
122+
" x3 = X(3)\n",
123+
" l1 = L(1)\n",
124+
" l2 = L(2)\n",
125+
"\n",
126+
" prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))\n",
127+
" odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n",
128+
" measurement_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2]))\n",
129+
"\n",
130+
" graph.addPriorPose2(x1, gtsam.Pose2(0.0, 0.0, 0.0), prior_noise)\n",
131+
" graph.add(gtsam.BetweenFactorPose2(x1, x2, gtsam.Pose2(2.0, 0.0, 0.0), odometry_noise))\n",
132+
" graph.add(gtsam.BetweenFactorPose2(x2, x3, gtsam.Pose2(2.0, 0.0, 0.0), odometry_noise))\n",
133+
"\n",
134+
" graph.add(gtsam.BearingRangeFactor2D(x1, l1, gtsam.Rot2.fromDegrees(45), np.sqrt(8.0), measurement_noise))\n",
135+
" graph.add(gtsam.BearingRangeFactor2D(x2, l1, gtsam.Rot2.fromDegrees(90), 2.0, measurement_noise))\n",
136+
" graph.add(gtsam.BearingRangeFactor2D(x3, l2, gtsam.Rot2.fromDegrees(90), 2.0, measurement_noise))\n",
137+
"\n",
138+
" initial = gtsam.Values()\n",
139+
" initial.insert(x1, gtsam.Pose2(0.1, -0.1, 0.05))\n",
140+
" initial.insert(x2, gtsam.Pose2(2.1, 0.1, -0.02))\n",
141+
" initial.insert(x3, gtsam.Pose2(4.2, -0.1, 0.04))\n",
142+
" initial.insert(l1, gtsam.Point2(1.8, 2.2))\n",
143+
" initial.insert(l2, gtsam.Point2(4.2, 2.1))\n",
144+
"\n",
145+
" return graph, initial, (x1, x2, x3, l1, l2)"
146+
]
147+
},
148+
{
149+
"cell_type": "code",
150+
"execution_count": 4,
151+
"id": "gtsam_nonlinear_doc_marginals__optimize",
152+
"metadata": {},
153+
"outputs": [],
154+
"source": [
155+
"graph, initial, (x1, x2, x3, l1, l2) = create_planar_slam_problem()\n",
156+
"\n",
157+
"params = gtsam.LevenbergMarquardtParams()\n",
158+
"optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)\n",
159+
"result = optimizer.optimize()\n",
160+
"\n",
161+
"marginals = gtsam.Marginals(graph, result)"
162+
]
163+
},
164+
{
165+
"cell_type": "markdown",
166+
"id": "gtsam_nonlinear_doc_marginals__single_variable",
167+
"metadata": {},
168+
"source": [
169+
"## Single-variable queries\n",
170+
"\n",
171+
"For one variable, `Marginals` can return either the covariance matrix or the information matrix."
172+
]
173+
},
174+
{
175+
"cell_type": "code",
176+
"execution_count": 5,
177+
"id": "gtsam_nonlinear_doc_marginals__single_queries",
178+
"metadata": {},
179+
"outputs": [
180+
{
181+
"name": "stdout",
182+
"output_type": "stream",
183+
"text": [
184+
"Covariance of x2:\n",
185+
" [[ 0.121 -0.0013 0.0045]\n",
186+
" [-0.0013 0.1584 0.0206]\n",
187+
" [ 0.0045 0.0206 0.0177]]\n",
188+
"Information of x2:\n",
189+
" [[ 8.3682 0.4077 -2.6045]\n",
190+
" [ 0.4077 7.4623 -8.7872]\n",
191+
" [-2.6045 -8.7872 67.2517]]\n"
192+
]
193+
}
194+
],
195+
"source": [
196+
"pose2_covariance = marginals.marginalCovariance(x2)\n",
197+
"pose2_information = marginals.marginalInformation(x2)\n",
198+
"\n",
199+
"print(\"Covariance of x2:\\n\", np.round(pose2_covariance, 4))\n",
200+
"print(\"Information of x2:\\n\", np.round(pose2_information, 4))"
201+
]
202+
},
203+
{
204+
"cell_type": "markdown",
205+
"id": "gtsam_nonlinear_doc_marginals__joint_queries",
206+
"metadata": {},
207+
"source": [
208+
"## Joint queries\n",
209+
"\n",
210+
"For several variables, `Marginals` returns a `JointMarginal`. You can inspect the full dense matrix, or retrieve individual covariance blocks by key."
211+
]
212+
},
213+
{
214+
"cell_type": "code",
215+
"execution_count": 6,
216+
"id": "gtsam_nonlinear_doc_marginals__joint_examples",
217+
"metadata": {},
218+
"outputs": [
219+
{
220+
"name": "stdout",
221+
"output_type": "stream",
222+
"text": [
223+
"Full joint covariance:\n",
224+
" [[ 0.1687 -0.0477 0.1029 -0.0439 -0.0265 0.1029 -0.0968 -0.0265]\n",
225+
" [-0.0477 0.1635 -0.0026 0.1468 0.0213 -0.0026 0.1894 0.0213]\n",
226+
" [ 0.1029 -0.0026 0.121 -0.0013 0.0045 0.121 0.0077 0.0045]\n",
227+
" [-0.0439 0.1468 -0.0013 0.1584 0.0206 -0.0013 0.1997 0.0206]\n",
228+
" [-0.0265 0.0213 0.0045 0.0206 0.0177 0.0045 0.0561 0.0177]\n",
229+
" [ 0.1029 -0.0026 0.121 -0.0013 0.0045 0.161 0.0077 0.0045]\n",
230+
" [-0.0968 0.1894 0.0077 0.1997 0.0561 0.0077 0.3519 0.0561]\n",
231+
" [-0.0265 0.0213 0.0045 0.0206 0.0177 0.0045 0.0561 0.0277]]\n",
232+
"Covariance block Cov[x2, l1]:\n",
233+
" [[ 0.1029 -0.0026]\n",
234+
" [-0.0439 0.1468]\n",
235+
" [-0.0265 0.0213]]\n",
236+
"Information block Info[x3, x3]:\n",
237+
" [[ 25. -0. -0.]\n",
238+
" [ -0. 25. -0.]\n",
239+
" [ -0. -0. 100.]]\n"
240+
]
241+
}
242+
],
243+
"source": [
244+
"joint_covariance = marginals.jointMarginalCovariance([x2, l1, x3])\n",
245+
"joint_information = marginals.jointMarginalInformation([x2, l1, x3])\n",
246+
"\n",
247+
"print(\"Full joint covariance:\\n\", np.round(joint_covariance.fullMatrix(), 4))\n",
248+
"print(\"Covariance block Cov[x2, l1]:\\n\", np.round(joint_covariance.at(x2, l1), 4))\n",
249+
"print(\"Information block Info[x3, x3]:\\n\", np.round(joint_information.at(x3, x3), 4))"
250+
]
251+
},
252+
{
253+
"cell_type": "markdown",
254+
"id": "gtsam_nonlinear_doc_marginals__performance",
255+
"metadata": {},
256+
"source": [
257+
"## Performance guidance\n",
258+
"\n",
259+
"Users normally do not need to manage Bayes trees directly when working with `Marginals`. GTSAM builds or reuses the Gaussian Bayes tree internally and exploits its structure when answering covariance queries.\n",
260+
"\n",
261+
"In practice, that means:\n",
262+
"\n",
263+
"- single-variable queries are already very efficient,\n",
264+
"- two-variable joint queries are also already localized and efficient, and\n",
265+
"- larger joint queries are now much faster than before because the internal support extraction is more selective.\n",
266+
"\n",
267+
"So the user-facing advice is simple: call the query you actually need, and let `Marginals` manage the Bayes-tree details."
268+
]
269+
},
270+
{
271+
"cell_type": "markdown",
272+
"id": "gtsam_nonlinear_doc_marginals__see_also",
273+
"metadata": {},
274+
"source": [
275+
"## See also\n",
276+
"\n",
277+
"- Blog post: [Fast Covariance Recovery in GTSAM Bayes Trees](https://gtsam.org/2026/03/29/bayes-tree-covariance-recovery.html)\n",
278+
"- Paper: [CovarianceRecovery.pdf](https://github.com/borglab/gtsam/blob/develop/doc/CovarianceRecovery.pdf)\n",
279+
"- Header: [`Marginals.h`](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/Marginals.h)"
280+
]
281+
}
282+
],
283+
"metadata": {
284+
"kernelspec": {
285+
"display_name": "py312",
286+
"language": "python",
287+
"name": "python3"
288+
},
289+
"language_info": {
290+
"codemirror_mode": {
291+
"name": "ipython",
292+
"version": 3
293+
},
294+
"file_extension": ".py",
295+
"mimetype": "text/x-python",
296+
"name": "python",
297+
"nbconvert_exporter": "python",
298+
"pygments_lexer": "ipython3",
299+
"version": "3.12.6"
300+
}
301+
},
302+
"nbformat": 4,
303+
"nbformat_minor": 5
304+
}

‎gtsam/nonlinear/nonlinear.md‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -49,5 +49,6 @@ The `nonlinear` module in GTSAM includes a comprehensive set of tools for nonlin
4949

5050
## Analysis and Visualization
5151

52-
- [Marginals](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/Marginals.h): Computes marginal covariances from optimization results.
53-
- [GraphvizFormatting](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/GraphvizFormatting.h): Provides customization for factor graph visualization.
52+
- [Marginals](doc/Marginals.ipynb): Computes marginal covariances and joint marginals from optimization results.
53+
- [`Marginals.h`](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/Marginals.h): C++ API declaration for `Marginals` and `JointMarginal`.
54+
- [GraphvizFormatting](https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/GraphvizFormatting.h): Provides customization for factor graph visualization.

0 commit comments

Comments
 (0)