Skip to content

Notebooks #2604

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
zcjhao wants to merge 9 commits into
base: develop
Choose a base branch
from
+4,863 −780
Open

Notebooks #2604

Show file tree
Hide file tree
Changes from 8 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
ebf1954
[Python] Add GPSFactorExample notebook
zcjhao Jul 22, 2026
22b8fab
[Python] Add OdometryExample notebook
zcjhao Jul 22, 2026
898378e
[Python] Add SimpleRotation notebook
zcjhao Jul 22, 2026
12a0141
[Python] Add FixedLagSmootherExample notebook
zcjhao Jul 22, 2026
9108255
[Python] Add VisualISAMExample notebook
zcjhao Jul 22, 2026
0e666bf
[Python] Add DogLegOptimizerExample notebook
zcjhao Jul 22, 2026
80823c4
[Python] Add SelfCalibrationExample notebook
zcjhao Jul 22, 2026
6c953ee
[Python] Add SFMExample notebook
zcjhao Jul 22, 2026
73fb669
[Python] Remove ported scripts and de-sequence notebook intros
zcjhao Aug 1, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
342 changes: 342 additions & 0 deletions python/gtsam/examples/DogLegOptimizerExample.ipynb
View file
Open in desktop

Large diffs are not rendered by default.

331 changes: 331 additions & 0 deletions python/gtsam/examples/FixedLagSmootherExample.ipynb
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,331 @@
{
"cells": [
{
"cell_type": "markdown",
"id": "dbdafeea",
"metadata": {},
"source": [
"# Fixed-Lag Smoother Example\n",
"\n",
"Every notebook so far has kept *every* variable in the problem forever. That doesn't scale to a robot that runs for hours: the graph, and the cost of solving it, would grow without bound.\n",
"\n",
"A **fixed-lag smoother** fixes this by only keeping variables whose timestamp falls within a trailing window (the \"lag\"). As new measurements arrive, anything older than the lag is automatically marginalized out -- summarized into the remaining variables and then dropped -- so the problem size stays bounded no matter how long the robot runs.\n",
"\n",
"The scenario: a robot drives in a straight line at 2 m/s. Two independent odometry-like sensors each measure the motion between consecutive poses (simulating sensor fusion), sampled every 0.25 seconds for 3 seconds, with a 2-second lag."
]
},
{
"cell_type": "markdown",
"id": "d1be22d7",
"metadata": {},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/FixedLagSmootherExample.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "markdown",
"id": "6d783a59",
"metadata": {},
"source": [
"GTSAM Copyright 2010-2026, Georgia Tech Research Corporation,\n",
"Atlanta, Georgia 30332-0415\n",
"All Rights Reserved\n",
"\n",
"Authors: Frank Dellaert (C++), Jeremy Aguilon (Python), et al. (see THANKS for the full author list)\n",
"\n",
"See LICENSE for the license information"
]
},
{
"cell_type": "code",
"execution_count": 1,
"id": "439cb4bd",
"metadata": {
"execution": {
"iopub.execute_input": "2026-07-22T10:34:49.014414Z",
"iopub.status.busy": "2026-07-22T10:34:49.014188Z",
"iopub.status.idle": "2026-07-22T10:34:49.019899Z",
"shell.execute_reply": "2026-07-22T10:34:49.019196Z"
}
},
"outputs": [],
"source": [
"try:\n",
" import google.colab\n",
" %pip install --quiet gtsam-develop\n",
"except ImportError:\n",
" pass"
]
},
{
"cell_type": "code",
"execution_count": 2,
"id": "e34ae8e2",
"metadata": {
"execution": {
"iopub.execute_input": "2026-07-22T10:34:49.022002Z",
"iopub.status.busy": "2026-07-22T10:34:49.021836Z",
"iopub.status.idle": "2026-07-22T10:34:49.205992Z",
"shell.execute_reply": "2026-07-22T10:34:49.205506Z"
}
},
"outputs": [],
"source": [
"import numpy as np\n",
"import gtsam"
]
},
{
"cell_type": "markdown",
"id": "90c2bdb5",
"metadata": {},
"source": [
"## 1. Configure the smoother\n",
"\n",
"`BatchFixedLagSmoother` re-solves with Levenberg-Marquardt on every `update()` call (GTSAM also has an `IncrementalFixedLagSmoother`, based on iSAM2, which is more efficient but not covered in this notebook). `lag=2.0` means only the most recent 2 seconds of poses are kept.\n",
"\n",
"Each update needs three staging containers: new factors, new variable values, and a mapping from key to timestamp -- the timestamps are how the smoother knows which variables have aged out of the window."
]
},
{
"cell_type": "code",
"execution_count": 3,
"id": "235c12ab",
"metadata": {
"execution": {
"iopub.execute_input": "2026-07-22T10:34:49.207298Z",
"iopub.status.busy": "2026-07-22T10:34:49.207189Z",
"iopub.status.idle": "2026-07-22T10:34:49.208950Z",
"shell.execute_reply": "2026-07-22T10:34:49.208656Z"
}
},
"outputs": [],
"source": [
"lag = 2.0\n",
"smoother_batch = gtsam.BatchFixedLagSmoother(lag)\n",
"\n",
"new_factors = gtsam.NonlinearFactorGraph()\n",
"new_values = gtsam.Values()\n",
"new_timestamps = {}"
]
},
{
"cell_type": "markdown",
"id": "fec4ec9f",
"metadata": {},
"source": [
"## 2. Prior on the first pose\n",
"\n",
"As in the odometry example, a prior anchors pose key `0` at the origin. We also record its timestamp as `0.0` seconds."
]
},
{
"cell_type": "code",
"execution_count": 4,
"id": "00d5938a",
"metadata": {
"execution": {
"iopub.execute_input": "2026-07-22T10:34:49.209955Z",
"iopub.status.busy": "2026-07-22T10:34:49.209902Z",
"iopub.status.idle": "2026-07-22T10:34:49.211928Z",
"shell.execute_reply": "2026-07-22T10:34:49.211611Z"
}
},
"outputs": [],
"source": [
"prior_mean = gtsam.Pose2(0, 0, 0)\n",
"prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))\n",
"X1 = 0\n",
"new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))\n",
"new_values.insert(X1, prior_mean)\n",
"new_timestamps[X1] = 0.0"
]
},
{
"cell_type": "markdown",
"id": "4d0bc0d0",
"metadata": {},
"source": [
"## 3. Simulate two odometry sensors over time\n",
"\n",
"Every 0.25 seconds we add a new pose key, derived directly from the timestamp (`int(1000 * time)`), and a constant-velocity guess (`time * 2` meters along x). Two `BetweenFactorPose2`s connect it to the previous pose -- one per \"sensor\", each with its own measurement and noise model, simulating fusion of two independently noisy odometry sources.\n",
"\n",
"`update()` is only called once every two steps (`time >= 0.50`), so the very first call batches two hops of factors at once, while later calls process one hop at a time. The staging containers are cleared after each `update()`, but the smoother's own internal window keeps sliding forward regardless."
]
},
{
"cell_type": "code",
"execution_count": 5,
"id": "f9fca920",
"metadata": {
"execution": {
"iopub.execute_input": "2026-07-22T10:34:49.212868Z",
"iopub.status.busy": "2026-07-22T10:34:49.212810Z",
"iopub.status.idle": "2026-07-22T10:34:49.220765Z",
"shell.execute_reply": "2026-07-22T10:34:49.220403Z"
}
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Timestamp = 0.5, Key = 500\n",
"(0.995821, 0.0231012, 0.0300001)\n",
"\n",
"Timestamp = 0.75, Key = 750\n",
"(1.49284, 0.0457247, 0.045)\n",
"\n",
"Timestamp = 1.0, Key = 1000\n",
"(1.98981, 0.0758879, 0.06)\n",
"\n",
"Timestamp = 1.25, Key = 1250\n",
"(2.48627, 0.113502, 0.075)\n",
"\n",
"Timestamp = 1.5, Key = 1500\n",
"(2.98211, 0.158558, 0.09)\n",
"\n",
"Timestamp = 1.75, Key = 1750\n",
"(3.47722, 0.211047, 0.105)\n",
"\n",
"Timestamp = 2.0, Key = 2000\n",
"(3.97149, 0.270956, 0.12)\n",
"\n",
"Timestamp = 2.25, Key = 2250\n",
"(4.4648, 0.338272, 0.135)\n",
"\n",
"Timestamp = 2.5, Key = 2500\n",
"(4.95705, 0.41298, 0.15)\n",
"\n",
"Timestamp = 2.75, Key = 2750\n",
"(5.44812, 0.495063, 0.165)\n",
"\n",
"Timestamp = 3.0, Key = 3000\n",
"(5.9379, 0.584503, 0.18)\n",
"\n"
]
}
],
"source": [
"delta_time = 0.25\n",
"time = 0.25\n",
"\n",
"while time <= 3.0:\n",
" previous_key = int(1000 * (time - delta_time))\n",
" current_key = int(1000 * time)\n",
"\n",
" # assign current key to the current timestamp\n",
" new_timestamps[current_key] = time\n",
"\n",
" # Add a guess for this pose to the new values\n",
" # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s]\n",
" current_pose = gtsam.Pose2(time * 2, 0, 0)\n",
" new_values.insert(current_key, current_pose)\n",
"\n",
" # Add odometry factors from two different sources with different error\n",
" # stats\n",
" odometry_measurement_1 = gtsam.Pose2(0.61, -0.08, 0.02)\n",
" odometry_noise_1 = gtsam.noiseModel.Diagonal.Sigmas(\n",
" np.array([0.1, 0.1, 0.05]))\n",
" new_factors.push_back(gtsam.BetweenFactorPose2(\n",
" previous_key, current_key, odometry_measurement_1, odometry_noise_1\n",
" ))\n",
"\n",
" odometry_measurement_2 = gtsam.Pose2(0.47, 0.03, 0.01)\n",
" odometry_noise_2 = gtsam.noiseModel.Diagonal.Sigmas(\n",
" np.array([0.05, 0.05, 0.05]))\n",
" new_factors.push_back(gtsam.BetweenFactorPose2(\n",
" previous_key, current_key, odometry_measurement_2, odometry_noise_2\n",
" ))\n",
"\n",
" # Update the smoothers with the new factors. In this case,\n",
" # one iteration must pass for Levenberg-Marquardt to accurately\n",
" # estimate\n",
" if time >= 0.50:\n",
" smoother_batch.update(new_factors, new_values, new_timestamps)\n",
" print(\"Timestamp = \" + str(time) + \", Key = \" + str(current_key))\n",
" print(smoother_batch.calculateEstimatePose2(current_key))\n",
"\n",
" new_timestamps.clear()\n",
" new_values.clear()\n",
" new_factors.resize(0)\n",
"\n",
" time += delta_time"
]
},
{
"cell_type": "markdown",
"id": "e39b6a7b",
"metadata": {},
"source": [
"## 4. What's kept in the window\n",
"\n",
"`smoother_batch.timestamps()` returns the key-to-timestamp map for every variable *currently* in the smoother. After 3 seconds have passed with a 2-second lag, pose `0` (timestamp `0.0`) should be long gone -- marginalized out, not just cosmetically hidden."
]
},
{
"cell_type": "code",
"execution_count": 6,
"id": "07f8a651",
"metadata": {
"execution": {
"iopub.execute_input": "2026-07-22T10:34:49.221672Z",
"iopub.status.busy": "2026-07-22T10:34:49.221622Z",
"iopub.status.idle": "2026-07-22T10:34:49.223375Z",
"shell.execute_reply": "2026-07-22T10:34:49.223009Z"
}
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Key: 1000 Time: 1.0\n",
"Key: 1250 Time: 1.25\n",
"Key: 1500 Time: 1.5\n",
"Key: 1750 Time: 1.75\n",
"Key: 2000 Time: 2.0\n",
"Key: 2250 Time: 2.25\n",
"Key: 2500 Time: 2.5\n",
"Key: 2750 Time: 2.75\n",
"Key: 3000 Time: 3.0\n"
]
}
],
"source": [
"remaining = smoother_batch.timestamps()\n",
"for key, t in sorted(remaining.items(), key=lambda kv: kv[1]):\n",
" print(f\"Key: {key} Time: {t}\")"
]
},
{
"cell_type": "markdown",
"id": "6b46f14e",
"metadata": {},
"source": [
"Only the poses from the last ~2 seconds remain; pose `0` and the earliest handful of poses are no longer part of the problem at all. This bounded-memory property is exactly why fixed-lag smoothing -- and its incremental, iSAM2-based cousin -- is the tool of choice for real-time or long-duration SLAM, where a robot cannot afford to keep re-optimizing its entire history forever."
]
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"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.13.12"
}
},
"nbformat": 4,
"nbformat_minor": 5
}
Loading
Loading