From 247172dae82f02bcd8d7bbcf84356d132f37a3b1 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 22 Apr 2025 23:35:23 -0400 Subject: [PATCH 1/9] Converted PlanarSLAM --- python/gtsam/examples/PlanarSLAMExample.ipynb | 508 ++++++++++++++++++ python/gtsam/examples/PlanarSLAMExample.py | 97 ---- 2 files changed, 508 insertions(+), 97 deletions(-) create mode 100644 python/gtsam/examples/PlanarSLAMExample.ipynb delete mode 100644 python/gtsam/examples/PlanarSLAMExample.py diff --git a/python/gtsam/examples/PlanarSLAMExample.ipynb b/python/gtsam/examples/PlanarSLAMExample.ipynb new file mode 100644 index 000000000..c03805665 --- /dev/null +++ b/python/gtsam/examples/PlanarSLAMExample.ipynb @@ -0,0 +1,508 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "153c8385", + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "source": [ + "GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information" + ] + }, + { + "cell_type": "markdown", + "id": "3ced5f44", + "metadata": { + "tags": [ + "remove-input" + ] + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "markdown", + "id": "d2980e5e", + "metadata": {}, + "source": [ + "# Simple Planar SLAM Example\n", + "\n", + "This notebook demonstrates a basic Simultaneous Localization and Mapping (SLAM) problem in 2D using GTSAM.\n", + "\n", + "**What is GTSAM?**\n", + "GTSAM (Georgia Tech Smoothing and Mapping) is a library that implements factor graph-based optimization. It's widely used in robotics for problems like SLAM, Structure from Motion (SfM), and sensor fusion.\n", + "\n", + "**What is a Factor Graph?**\n", + "A factor graph is a graphical model that represents the probabilistic relationships between unknown variables (like robot poses and landmark positions) and measurements (like odometry or sensor readings). Optimization in GTSAM involves finding the most likely configuration of variables given the measurements and their associated uncertainties (noise).\n", + "\n", + "**This Example:**\n", + "We'll simulate a robot moving in a 2D plane. The robot has:\n", + "1. **Odometry:** Measurements of its own motion (how far it moved between steps).\n", + "2. **Bearing-Range Sensor:** A sensor (like a simple laser scanner) that measures the bearing (angle) and range (distance) to landmarks.\n", + "\n", + "We'll build a factor graph representing the robot's poses, landmark positions, odometry measurements, and bearing-range measurements. Then, we'll use GTSAM to optimize the graph and find the best estimate of the robot's trajectory and landmark locations." + ] + }, + { + "cell_type": "markdown", + "id": "9eb7fe9d", + "metadata": {}, + "source": [ + "## 1. Setup and Imports\n", + "\n", + "First, we need to import the necessary libraries: `gtsam` itself and `numpy` for numerical operations." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "eea967e9", + "metadata": {}, + "outputs": [], + "source": [ + "# Install GTSAM from pip if running in Google Colab\n", + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not in Colab" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2c932acb", + "metadata": {}, + "outputs": [], + "source": [ + "import matplotlib.pyplot as plt\n", + "import numpy as np\n", + "import graphviz\n", + "\n", + "import gtsam\n", + "import gtsam.utils.plot as gp\n", + "\n", + "# We can use shorthand symbols for variable keys\n", + "# X(i) represents the i-th pose variable\n", + "# L(j) represents the j-th landmark variable\n", + "from gtsam.symbol_shorthand import L, X" + ] + }, + { + "cell_type": "markdown", + "id": "181c929c", + "metadata": {}, + "source": [ + "## 2. Define Noise Models\n", + "\n", + "Real-world measurements are always noisy. In GTSAM, we model this noise using Gaussian noise models. We need to define the uncertainty (standard deviation) for each type of measurement:\n", + "\n", + "* **Prior Noise:** Uncertainty about the very first pose of the robot. We often assume we know the starting position reasonably well, but not perfectly.\n", + "* **Odometry Noise:** Uncertainty in the robot's movement measurements (e.g., wheel encoders). Assumed to be Gaussian noise on the change in x, y, and theta.\n", + "* **Measurement Noise:** Uncertainty in the bearing-range sensor readings. Assumed to be Gaussian noise on the bearing and range measurements." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "4a9b8d1b", + "metadata": {}, + "outputs": [], + "source": [ + "# Create noise models with specified standard deviations (sigmas).\n", + "# gtsam.noiseModel.Diagonal.Sigmas takes a numpy array of standard deviations.\n", + "\n", + "# Prior noise on the first pose (x, y, theta) - sigmas = [0.3m, 0.3m, 0.1rad]\n", + "PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))\n", + "# Odometry noise (dx, dy, dtheta) - sigmas = [0.2m, 0.2m, 0.1rad]\n", + "ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n", + "# Measurement noise (bearing, range) - sigmas = [0.1rad, 0.2m]\n", + "MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2]))" + ] + }, + { + "cell_type": "markdown", + "id": "6202026a", + "metadata": {}, + "source": [ + "## 3. Build the Factor Graph\n", + "\n", + "Now, we'll create the factor graph step-by-step.\n", + "\n", + "First, create an empty nonlinear factor graph." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "37e5a43a", + "metadata": {}, + "outputs": [], + "source": [ + "# Create an empty nonlinear factor graph\n", + "graph = gtsam.NonlinearFactorGraph()" + ] + }, + { + "cell_type": "markdown", + "id": "8a1b517b", + "metadata": {}, + "source": [ + "### 3.1 Variable Keys\n", + "\n", + "We need unique keys to identify each unknown variable (robot poses and landmark positions) in the graph. \n", + "We'll have 3 robot poses $(x_1, x_2, x_3)$ and 2 landmarks $(l_1, l_2)$. We will just use `X(1)`..`L(2)` in the code, using the `X(i)` and `L(j)` shorthand imported earlier. Note here we can use base 1 indexing but you can use any integer, and numbering does not have to be consecutive." + ] + }, + { + "cell_type": "markdown", + "id": "d9e1aaaa", + "metadata": {}, + "source": [ + "### 3.2 Add a Prior Factor\n", + "\n", + "To \"anchor\" the graph, we add a prior factor on the first pose $x_1$. This represents our initial belief about where the robot started. We assume it started at the origin (0, 0, 0) with the uncertainty defined by `PRIOR_NOISE`.\n", + "\n", + "A `PriorFactorPose2` connects the single pose variable $x_1$ to a known pose (`gtsam.Pose2(0,0,0)`) with a specific noise model." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "0549b0a2", + "metadata": {}, + "outputs": [], + "source": [ + "# Add a prior on pose X(1) at the origin.\n", + "# A prior factor consists of a mean (gtsam.Pose2) and a noise model.\n", + "graph.add(gtsam.PriorFactorPose2(X(1), gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))" + ] + }, + { + "cell_type": "markdown", + "id": "cb8ffd2d", + "metadata": {}, + "source": [ + "### 3.3 Add Odometry Factors\n", + "\n", + "Next, we add factors representing the robot's movement based on odometry measurements. We assume the robot moved approximately 2 units forward in the x-direction between each pose.\n", + "\n", + "A `BetweenFactorPose2` connects two consecutive poses (e.g., $x_1$ and $x_2$) and represents the measured relative motion between them (`gtsam.Pose2(2.0, 0.0, 0.0)`) with its associated noise (`ODOMETRY_NOISE`)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2389fae3", + "metadata": {}, + "outputs": [], + "source": [ + "# Add odometry factors between X(1),X(2) and X(2),X(3), respectively.\n", + "# The measurement is the relative motion: Pose2(dx, dy, dtheta).\n", + "\n", + "# Between X(1) and X(2): Move forward 2m\n", + "graph.add(gtsam.BetweenFactorPose2(X(1), X(2), gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))\n", + "# Between X(2) and X(3): Move forward 2m\n", + "graph.add(gtsam.BetweenFactorPose2(X(2), X(3), gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))" + ] + }, + { + "cell_type": "markdown", + "id": "17459992", + "metadata": {}, + "source": [ + "### 3.4 Add Measurement Factors\n", + "\n", + "Now, add factors representing the bearing-range measurements from the robot's poses to the landmarks.\n", + "\n", + "A `BearingRangeFactor2D` connects a pose variable (e.g., $x_1$) and a landmark variable (e.g., $l_1$). It includes the measured bearing (`gtsam.Rot2`) and range (distance), along with the measurement noise (`MEASUREMENT_NOISE`).\n", + "\n", + "We have three measurements:\n", + "* From $x_1$ to $l_1$: Bearing 45 degrees, Range sqrt(8)\n", + "* From $x_2$ to $l_1$: Bearing 90 degrees, Range 2.0\n", + "* From $x_3$ to $l_2$: Bearing 90 degrees, Range 2.0" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "cc19f4ac", + "metadata": {}, + "outputs": [], + "source": [ + "# Add Range-Bearing measurements to two different landmarks L(1) and L(2).\n", + "# Measurements are Bearing (gtsam.Rot2) and Range (float).\n", + "\n", + "# From X(1) to L(1)\n", + "graph.add(gtsam.BearingRangeFactor2D(X(1), L(1), gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE))\n", + "# From X(2) to L(1)\n", + "graph.add(gtsam.BearingRangeFactor2D(X(2), L(1), gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))\n", + "# From X(3) to L(2)\n", + "graph.add(gtsam.BearingRangeFactor2D(X(3), L(2), gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))" + ] + }, + { + "cell_type": "markdown", + "id": "4820f7b1", + "metadata": {}, + "source": [ + "### 3.5 Inspect the Graph\n", + "\n", + "We can print the factor graph to see a summary of the variables and factors we've added." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "83b8002e", + "metadata": {}, + "outputs": [], + "source": [ + "# Print the graph. This shows the factors and the variables they connect.\n", + "print(\"Factor Graph:\\n{}\".format(graph))" + ] + }, + { + "cell_type": "markdown", + "id": "038f5089", + "metadata": {}, + "source": [ + "## 4. Create Initial Estimate\n", + "\n", + "Factor graph optimization is an iterative process that needs an initial guess (initial estimate) for the values of all unknown variables. The closer the initial estimate is to the true solution, the faster and more reliably the optimizer will converge.\n", + "\n", + "Here, we create a `gtsam.Values` object and deliberately insert slightly *inaccurate* initial estimates for the poses and landmark positions. This demonstrates that the optimizer can correct for errors in the initial guess." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "98c87675", + "metadata": {}, + "outputs": [], + "source": [ + "# Create (deliberately inaccurate) initial estimate.\n", + "# gtsam.Values is a container mapping variable keys to their estimated values.\n", + "initial_estimate = gtsam.Values()\n", + "\n", + "# Insert initial guesses for poses (Pose2: x, y, theta)\n", + "initial_estimate.insert(X(1), gtsam.Pose2(-0.25, 0.20, 0.15))\n", + "initial_estimate.insert(X(2), gtsam.Pose2(2.30, 0.10, -0.20))\n", + "initial_estimate.insert(X(3), gtsam.Pose2(4.10, 0.10, 0.10))\n", + "\n", + "# Insert initial guesses for landmarks (Point2: x, y)\n", + "initial_estimate.insert(L(1), gtsam.Point2(1.80, 2.10))\n", + "initial_estimate.insert(L(2), gtsam.Point2(4.10, 1.80))\n", + "\n", + "# Print the initial estimate\n", + "print(\"Initial Estimate:\\n{}\".format(initial_estimate))" + ] + }, + { + "cell_type": "markdown", + "id": "2d796916", + "metadata": {}, + "source": [ + "Now that we have an initial estimate we can also visualize the graph:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d896ecee", + "metadata": {}, + "outputs": [], + "source": [ + "display(graphviz.Source(graph.dot(initial_estimate)))" + ] + }, + { + "cell_type": "markdown", + "id": "2608bf96", + "metadata": {}, + "source": [ + "## 5. Optimize the Factor Graph\n", + "\n", + "Now, we use an optimizer to find the variable configuration that best fits all the factors (measurements) in the graph, starting from the initial estimate.\n", + "\n", + "We'll use the Levenberg-Marquardt (LM) algorithm, a standard non-linear least-squares optimizer.\n", + "\n", + "1. Create LM parameters (`gtsam.LevenbergMarquardtParams`). We'll use the defaults.\n", + "2. Create the optimizer instance, providing the graph, initial estimate, and parameters.\n", + "3. Run the optimization by calling `optimizer.optimize()`." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "2ee6b17a", + "metadata": {}, + "outputs": [], + "source": [ + "# Optimize using Levenberg-Marquardt optimization.\n", + "# The optimizer accepts optional parameters, but we'll use the defaults here.\n", + "params = gtsam.LevenbergMarquardtParams()\n", + "optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params)\n", + "\n", + "# Perform the optimization\n", + "result = optimizer.optimize()\n", + "\n", + "# Print the final optimized result\n", + "# This gtsam.Values object contains the most likely estimates for all variables.\n", + "print(\"\\nFinal Result:\\n{}\".format(result))" + ] + }, + { + "cell_type": "markdown", + "id": "1bda91e1", + "metadata": {}, + "source": [ + "The code below visualizes the optimized poses and landmarks in 2D. It uses GTSAM's plotting utilities to plot the robot's trajectory (poses) and the estimated positions of the landmarks. The poses are represented as coordinate frames indicating the robot's orientation, while the landmarks are plotted as blue points. The aspect ratio is set to ensure equal scaling for both axes." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d827195e", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(1)\n", + "axes = fig.add_subplot()\n", + "axes = fig.axes[0]\n", + "\n", + "# Plot 2D poses\n", + "poses : gtsam.Values = gtsam.utilities.allPose2s(result)\n", + "for key in poses.keys():\n", + " pose = poses.atPose2(key)\n", + " gp.plot_pose2_on_axes(axes, pose, axis_length=0.3)\n", + "\n", + "# Plot 2D landmarks\n", + "landmarks : np.ndarray = gtsam.utilities.extractPoint2(result) # 2xn array\n", + "for landmark in landmarks:\n", + " gp.plot_point2_on_axes(axes, landmark, linespec=\"b\")\n", + "\n", + "axes.set_aspect(\"equal\", adjustable=\"datalim\")" + ] + }, + { + "cell_type": "markdown", + "id": "27bc8e38", + "metadata": {}, + "source": [ + "## 6. Calculate Marginal Covariances\n", + "\n", + "Besides finding the optimal values (the mean), GTSAM can also compute the uncertainty (covariance) associated with each variable estimate after optimization. This tells us how confident we are about the estimated poses and landmark locations.\n", + "\n", + "We use the `gtsam.Marginals` class to calculate the marginal covariance matrices for each variable.\n", + "Each pose covariance matrix is a 3x3 matrix because a pose in 2D (`Pose2`) has three components: `(x, y, theta)` Each landmark covariance matrix is a 2x2 matrix because a landmark in 2D is represented by its `(x, y)` position.\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "90ef96ff", + "metadata": {}, + "outputs": [], + "source": [ + "# Calculate and print marginal covariances for all variables.\n", + "# This provides information about the uncertainty of the estimates.\n", + "marginals = gtsam.Marginals(graph, result)\n", + "\n", + "# Print the covariance matrix for each variable\n", + "print(\"X1 covariance:\\n{}\\n\".format(marginals.marginalCovariance(X(1))))\n", + "print(\"X2 covariance:\\n{}\\n\".format(marginals.marginalCovariance(X(2))))\n", + "print(\"X3 covariance:\\n{}\\n\".format(marginals.marginalCovariance(X(3))))\n", + "print(\"L1 covariance:\\n{}\\n\".format(marginals.marginalCovariance(L(1))))\n", + "print(\"L2 covariance:\\n{}\\n\".format(marginals.marginalCovariance(L(2))))" + ] + }, + { + "cell_type": "markdown", + "id": "62fd4f35", + "metadata": {}, + "source": [ + "The code below once again visualizes the optimized poses and landmarks, now with their associated uncertainties (covariances).\n", + "The covariance ellipses plotted on the graph visually represent the uncertainty in the estimates. Larger ellipses indicate higher uncertainty, while smaller ellipses indicate more confident estimates. \n", + "\n", + "The prior is on $x_1$, at the origin, and hence that is the most certain pose, after which uncertainty increases. Note that for poses we only show the uncertainty on translation, although each pose also has an uncertain orientation. The covariance ellipses on the landmarks actually reflect that orientation uncertainty, being oriented the way they are." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "d1f03fee", + "metadata": {}, + "outputs": [], + "source": [ + "fig = plt.figure(2)\n", + "axes = fig.add_subplot()\n", + "axes = fig.axes[0]\n", + "\n", + "# Plot 2D poses\n", + "poses = gtsam.utilities.allPose2s(result)\n", + "for key in poses.keys():\n", + " pose = poses.atPose2(key)\n", + " covariance = marginals.marginalCovariance(key)\n", + "\n", + " gp.plot_pose2_on_axes(axes, pose, covariance=covariance, axis_length=0.3)\n", + "\n", + "# Plot 2D landmarks\n", + "landmarks: np.ndarray = gtsam.utilities.extractPoint2(result) # 2xn array\n", + "for j, landmark in enumerate(landmarks):\n", + " gp.plot_point2_on_axes(axes, landmark, linespec=\"b\")\n", + " covariance = marginals.marginalCovariance(L(j+1))\n", + " gp.plot_covariance_ellipse_2d(axes, landmark, covariance=covariance)\n", + "\n", + "axes.set_aspect(\"equal\", adjustable=\"datalim\")" + ] + }, + { + "cell_type": "markdown", + "id": "74673b3d", + "metadata": {}, + "source": [ + "## Summary\n", + "\n", + "We have successfully:\n", + "1. Defined the structure of a 2D SLAM problem using factors (prior, odometry, measurements) and variables (poses, landmarks).\n", + "2. Represented this problem as a `gtsam.NonlinearFactorGraph`.\n", + "3. Provided noisy measurements and an inaccurate initial estimate.\n", + "4. Used `gtsam.LevenbergMarquardtOptimizer` to find the most likely configuration of poses and landmarks.\n", + "5. Calculated the uncertainty (covariance) of the final estimates.\n", + "\n", + "This demonstrates the basic workflow of using GTSAM for solving SLAM and other robotics estimation problems." + ] + } + ], + "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 +} diff --git a/python/gtsam/examples/PlanarSLAMExample.py b/python/gtsam/examples/PlanarSLAMExample.py deleted file mode 100644 index d2ee92c95..000000000 --- a/python/gtsam/examples/PlanarSLAMExample.py +++ /dev/null @@ -1,97 +0,0 @@ -""" -GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved -Authors: Frank Dellaert, et al. (see THANKS for the full author list) - -See LICENSE for the license information - -Simple robotics example using odometry measurements and bearing-range (laser) measurements -Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) -""" -# pylint: disable=invalid-name, E1101 - -from __future__ import print_function - -import gtsam -import numpy as np -from gtsam.symbol_shorthand import L, X - -# Create noise models -PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1])) -ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) -MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2])) - - -def main(): - """Main runner""" - - # Create an empty nonlinear factor graph - graph = gtsam.NonlinearFactorGraph() - - # Create the keys corresponding to unknown variables in the factor graph - X1 = X(1) - X2 = X(2) - X3 = X(3) - L1 = L(4) - L2 = L(5) - - # Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model - graph.add( - gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE)) - - # Add odometry factors between X1,X2 and X2,X3, respectively - graph.add( - gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), - ODOMETRY_NOISE)) - graph.add( - gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), - ODOMETRY_NOISE)) - - # Add Range-Bearing measurements to two different landmarks L1 and L2 - graph.add( - gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45), - np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE)) - graph.add( - gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, - MEASUREMENT_NOISE)) - graph.add( - gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, - MEASUREMENT_NOISE)) - - # Print graph - print("Factor Graph:\n{}".format(graph)) - - # Create (deliberately inaccurate) initial estimate - initial_estimate = gtsam.Values() - initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15)) - initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20)) - initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10)) - initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10)) - initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80)) - - # Print - print("Initial Estimate:\n{}".format(initial_estimate)) - - # Optimize using Levenberg-Marquardt optimization. The optimizer - # accepts an optional set of configuration parameters, controlling - # things like convergence criteria, the type of linear system solver - # to use, and the amount of information displayed during optimization. - # Here we will use the default set of parameters. See the - # documentation for the full set of parameters. - params = gtsam.LevenbergMarquardtParams() - optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, - params) - result = optimizer.optimize() - print("\nFinal Result:\n{}".format(result)) - - # Calculate and print marginal covariances for all variables - marginals = gtsam.Marginals(graph, result) - for (key, s) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"), - (L2, "L2")]: - print("{} covariance:\n{}\n".format(s, - marginals.marginalCovariance(key))) - - -if __name__ == "__main__": - main() From e52685d4412f201264026d94e5a7dd62c76dde82 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 22 Apr 2025 23:55:42 -0400 Subject: [PATCH 2/9] Pose-SLAM and rendering --- gtsam/inference/doc/Shortcuts.ipynb | 391 ++++++++++ python/gtsam/examples/PlanarSLAMExample.ipynb | 352 ++++++++- python/gtsam/examples/Pose2SLAMExample.ipynb | 712 ++++++++++++++++++ python/gtsam/examples/Pose2SLAMExample.py | 102 --- 4 files changed, 1431 insertions(+), 126 deletions(-) create mode 100644 gtsam/inference/doc/Shortcuts.ipynb create mode 100644 python/gtsam/examples/Pose2SLAMExample.ipynb delete mode 100644 python/gtsam/examples/Pose2SLAMExample.py diff --git a/gtsam/inference/doc/Shortcuts.ipynb b/gtsam/inference/doc/Shortcuts.ipynb new file mode 100644 index 000000000..66dd0bcf2 --- /dev/null +++ b/gtsam/inference/doc/Shortcuts.ipynb @@ -0,0 +1,391 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Efficent Marginals Computation\n", + "\n", + "GTSAM can very efficiently calculate marginals in Bayes trees. In this post, we illustrate the “shortcut” mechanism for **caching** the conditional distribution $P(S \\mid R)$ in a Bayes tree, allowing efficient other marginal queries. We assume familiarity with **Bayes trees** from [the previous post](#)." + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Toy Example\n", + "\n", + "We create a small Bayes tree:\n", + "\n", + "\\begin{equation}\n", + "P(a \\mid b) P(b,c \\mid r) P(f \\mid e) P(d,e \\mid r) P(r).\n", + "\\end{equation}\n", + "\n", + "Below is some Python code (using GTSAM’s discrete wrappers) to define and build the corresponding Bayes tree. We'll use a discrete example, i.e., we'll create a `DiscreteBayesTree`.\n" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "from gtsam import DiscreteConditional, DiscreteBayesTree, DiscreteBayesTreeClique, DecisionTreeFactor" + ] + }, + { + "cell_type": "code", + "execution_count": 2, + "metadata": {}, + "outputs": [], + "source": [ + "# Make discrete keys (key in elimination order, cardinality):\n", + "keys = [(0, 2), (1, 2), (2, 2), (3, 2), (4, 2), (5, 2), (6, 2)]\n", + "names = {0: 'a', 1: 'f', 2: 'b', 3: 'c', 4: 'd', 5: 'e', 6: 'r'}\n", + "aKey, fKey, bKey, cKey, dKey, eKey, rKey = keys\n", + "keyFormatter = lambda key: names[key]" + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "metadata": {}, + "outputs": [], + "source": [ + "# 1. Root Clique: P(r)\n", + "cliqueR = DiscreteBayesTreeClique(DiscreteConditional(rKey, \"0.4/0.6\"))\n", + "\n", + "# 2. Child Clique 1: P(b, c | r)\n", + "cliqueBC = DiscreteBayesTreeClique(\n", + " DiscreteConditional(\n", + " 2, DecisionTreeFactor([bKey, cKey, rKey], \"0.3 0.7 0.1 0.9 0.2 0.8 0.4 0.6\")\n", + " )\n", + ")\n", + "\n", + "# 3. Child Clique 2: P(d, e | r)\n", + "cliqueDE = DiscreteBayesTreeClique(\n", + " DiscreteConditional(\n", + " 2, DecisionTreeFactor([dKey, eKey, rKey], \"0.1 0.9 0.9 0.1 0.2 0.8 0.3 0.7\")\n", + " )\n", + ")\n", + "\n", + "# 4. Leaf Clique from Child 1: P(a | b)\n", + "cliqueA = DiscreteBayesTreeClique(DiscreteConditional(aKey, [bKey], \"1/3 3/1\"))\n", + "\n", + "# 5. Leaf Clique from Child 2: P(f | e)\n", + "cliqueF = DiscreteBayesTreeClique(DiscreteConditional(fKey, [eKey], \"1/3 3/1\"))" + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "metadata": {}, + "outputs": [], + "source": [ + "# Build the BayesTree:\n", + "bayesTree = DiscreteBayesTree()\n", + "\n", + "# Insert root:\n", + "bayesTree.insertRoot(cliqueR)\n", + "\n", + "# Attach child cliques to root:\n", + "bayesTree.addClique(cliqueBC, cliqueR)\n", + "bayesTree.addClique(cliqueDE, cliqueR)\n", + "\n", + "# Attach leaf cliques:\n", + "bayesTree.addClique(cliqueA, cliqueBC)\n", + "bayesTree.addClique(cliqueF, cliqueDE)\n", + "\n", + "# bayesTree.print(\"bayesTree\", keyFormatter)" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "metadata": {}, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "G\n", + "\n", + "\n", + "\n", + "0\n", + "\n", + "r\n", + "\n", + "\n", + "\n", + "1\n", + "\n", + "b, c : r\n", + "\n", + "\n", + "\n", + "0->1\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "3\n", + "\n", + "d, e : r\n", + "\n", + "\n", + "\n", + "0->3\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "2\n", + "\n", + "a : b\n", + "\n", + "\n", + "\n", + "1->2\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "4\n", + "\n", + "f : e\n", + "\n", + "\n", + "\n", + "3->4\n", + "\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "execution_count": 5, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "import graphviz\n", + "graphviz.Source(bayesTree.dot(keyFormatter))" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Naive Computation of P(a)\n", + "The marginal $P(a)$ can be computed by summing out the other variables in the tree:\n", + "$$\n", + "P(a) = \\sum_{b, c, d, e, f, r} P(a, b, c, d, e, f, r)\n", + "$$\n", + "\n", + "Using the Bayes tree structure, we have\n", + "\n", + "$$\n", + "P(a) = \\sum_{b, c, d, e, f, r} P(a \\mid b) P(b, c \\mid r) P(f \\mid e) P(d, e \\mid r) P(r) \n", + "$$\n", + "\n", + "but we can ignore variables $e$ and $f$ not on the path from $a$ to the root $r$. Indeed, by associativity we have\n", + "\n", + "$$\n", + "P(a) = \\sum_{r} \\Bigl\\{ \\sum_{e,f} P(f \\mid e) P(d, e \\mid r) \\Bigr\\} \\sum_{b, c, d} P(a \\mid b) P(b, c \\mid r) P(r)\n", + "$$\n", + "\n", + "where the grouped terms sum to one for any value of $r$, and hence\n", + "\n", + "$$\n", + "P(a) = \\sum_{r, b, c, d} P(a \\mid b) P(b, c \\mid r) P(r).\n", + "$$" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "## Memoization via Shortcuts\n", + "\n", + "In GTSAM, we compute this recursively\n", + "\n", + "#### Step 1\n", + "We want to compute the marginal via\n", + "$$\n", + "P(a) = \\sum_{r, b} P(a \\mid b) P(b).\n", + "$$\n", + "where $P(b)$ is the separator of this clique.\n", + "\n", + "#### Step 2\n", + "To compute the separator marginal, we use the **shortcut** $P(b|r)$:\n", + "$$\n", + "P(b) = \\sum_{r} P(b \\mid r) P(r).\n", + "$$\n", + "In general, a shortcut $P(S|R)$ directly conditions this clique's separator $S$ on the root clique $R$, even if there are many other cliques in-between. That is why it is called a *shortcut*.\n", + "\n", + "#### Step 3 (optional)\n", + "If the shortcut was already computed, then we are done! If not, we compute it recursively:\n", + "$$\n", + "P(S\\mid R) = \\sum_{F_p,\\,S_p \\setminus S}P(F_p \\mid S_p) P(S_p \\mid R).\n", + "$$\n", + "Above $P(F_p \\mid S_p)$ is the parent clique, and by the running intersection property we know that the seprator $S$ is a subset of the parent clique's variables.\n", + "Note that the recursion is because we might not have $P(S_p \\mid R)$ yet, so it might have to be computed in turn, etc. The recursion ends at nodes below the root, and **after we have obtained $P(S\\mid R)$ we cache it**.\n", + "\n", + "In our example, the computation is simply\n", + "$$\n", + "P(b|r) = \\sum_{c} P(b, c \\mid r),\n", + "$$\n", + "because this the parent separator is already the root, so $P(S_p \\mid R)$ is omitted. This is also the end of the recursion.\n" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "0\n", + "Marginal P(a):\n", + " Discrete Conditional\n", + " P( 0 ):\n", + " Choice(0) \n", + " 0 Leaf 0.51\n", + " 1 Leaf 0.49\n", + "\n", + "\n", + "3\n" + ] + } + ], + "source": [ + "# Marginal of the leaf variable 'a':\n", + "print(bayesTree.numCachedSeparatorMarginals())\n", + "marg_a = bayesTree.marginalFactor(aKey[0])\n", + "print(\"Marginal P(a):\\n\", marg_a)\n", + "print(bayesTree.numCachedSeparatorMarginals())\n" + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "3\n", + "Marginal P(b):\n", + " Discrete Conditional\n", + " P( 2 ):\n", + " Choice(2) \n", + " 0 Leaf 0.48\n", + " 1 Leaf 0.52\n", + "\n", + "\n", + "3\n" + ] + } + ], + "source": [ + "\n", + "# Marginal of the internal variable 'b':\n", + "print(bayesTree.numCachedSeparatorMarginals())\n", + "marg_b = bayesTree.marginalFactor(bKey[0])\n", + "print(\"Marginal P(b):\\n\", marg_b)\n", + "print(bayesTree.numCachedSeparatorMarginals())\n" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "3\n", + "Joint P(a, f):\n", + " DiscreteBayesNet\n", + " \n", + "size: 2\n", + "conditional 0: P( 0 | 1 ):\n", + " Choice(1) \n", + " 0 Choice(0) \n", + " 0 0 Leaf 0.51758893\n", + " 0 1 Leaf 0.48241107\n", + " 1 Choice(0) \n", + " 1 0 Leaf 0.50222672\n", + " 1 1 Leaf 0.49777328\n", + "\n", + "conditional 1: P( 1 ):\n", + " Choice(1) \n", + " 0 Leaf 0.506\n", + " 1 Leaf 0.494\n", + "\n", + "\n", + "3\n" + ] + } + ], + "source": [ + "\n", + "# Joint of leaf variables 'a' and 'f': P(a, f)\n", + "# This effectively needs to gather info from two different branches\n", + "print(bayesTree.numCachedSeparatorMarginals())\n", + "marg_af = bayesTree.jointBayesNet(aKey[0], fKey[0])\n", + "print(\"Joint P(a, f):\\n\", marg_af)\n", + "print(bayesTree.numCachedSeparatorMarginals())\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "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 +} diff --git a/python/gtsam/examples/PlanarSLAMExample.ipynb b/python/gtsam/examples/PlanarSLAMExample.ipynb index c03805665..e96c6ed2c 100644 --- a/python/gtsam/examples/PlanarSLAMExample.ipynb +++ b/python/gtsam/examples/PlanarSLAMExample.ipynb @@ -1,5 +1,13 @@ { "cells": [ + { + "cell_type": "markdown", + "id": "21db97de", + "metadata": {}, + "source": [ + "# Simple Planar SLAM Example" + ] + }, { "cell_type": "markdown", "id": "153c8385", @@ -14,7 +22,10 @@ "All Rights Reserved\n", "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", "\n", - "See LICENSE for the license information" + "See LICENSE for the license information\n", + "\n", + "Simple robotics example using odometry measurements and bearing-range (laser) measurements\n", + "Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)" ] }, { @@ -34,8 +45,6 @@ "id": "d2980e5e", "metadata": {}, "source": [ - "# Simple Planar SLAM Example\n", - "\n", "This notebook demonstrates a basic Simultaneous Localization and Mapping (SLAM) problem in 2D using GTSAM.\n", "\n", "**What is GTSAM?**\n", @@ -64,7 +73,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 15, "id": "eea967e9", "metadata": {}, "outputs": [], @@ -79,7 +88,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 16, "id": "2c932acb", "metadata": {}, "outputs": [], @@ -113,7 +122,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 17, "id": "4a9b8d1b", "metadata": {}, "outputs": [], @@ -143,7 +152,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 18, "id": "37e5a43a", "metadata": {}, "outputs": [], @@ -177,7 +186,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 19, "id": "0549b0a2", "metadata": {}, "outputs": [], @@ -201,7 +210,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 20, "id": "2389fae3", "metadata": {}, "outputs": [], @@ -234,7 +243,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 21, "id": "cc19f4ac", "metadata": {}, "outputs": [], @@ -262,10 +271,51 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 22, "id": "83b8002e", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Factor Graph:\n", + "NonlinearFactorGraph: size: 6\n", + "\n", + "Factor 0: PriorFactor on x1\n", + " prior mean: (0, 0, 0)\n", + " noise model: diagonal sigmas [0.3; 0.3; 0.1];\n", + "\n", + "Factor 1: BetweenFactor(x1,x2)\n", + " measured: (2, 0, 0)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 2: BetweenFactor(x2,x3)\n", + " measured: (2, 0, 0)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 3: BearingRangeFactor\n", + "Factor 3: keys = { x1 l1 }\n", + " noise model: diagonal sigmas [0.1; 0.2];\n", + "ExpressionFactor with measurement: bearing : 0.785398163\n", + "range 2.82842712\n", + "\n", + "Factor 4: BearingRangeFactor\n", + "Factor 4: keys = { x2 l1 }\n", + " noise model: diagonal sigmas [0.1; 0.2];\n", + "ExpressionFactor with measurement: bearing : 1.57079633\n", + "range 2\n", + "\n", + "Factor 5: BearingRangeFactor\n", + "Factor 5: keys = { x3 l2 }\n", + " noise model: diagonal sigmas [0.1; 0.2];\n", + "ExpressionFactor with measurement: bearing : 1.57079633\n", + "range 2\n", + "\n", + "\n" + ] + } + ], "source": [ "# Print the graph. This shows the factors and the variables they connect.\n", "print(\"Factor Graph:\\n{}\".format(graph))" @@ -285,10 +335,41 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 23, "id": "98c87675", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Initial Estimate:\n", + "Values with 5 values:\n", + "Value l1: (Eigen::Matrix)\n", + "[\n", + "\t1.8;\n", + "\t2.1\n", + "]\n", + "\n", + "Value l2: (Eigen::Matrix)\n", + "[\n", + "\t4.1;\n", + "\t1.8\n", + "]\n", + "\n", + "Value x1: (gtsam::Pose2)\n", + "(-0.25, 0.2, 0.15)\n", + "\n", + "Value x2: (gtsam::Pose2)\n", + "(2.3, 0.1, -0.2)\n", + "\n", + "Value x3: (gtsam::Pose2)\n", + "(4.1, 0.1, 0.1)\n", + "\n", + "\n" + ] + } + ], "source": [ "# Create (deliberately inaccurate) initial estimate.\n", "# gtsam.Values is a container mapping variable keys to their estimated values.\n", @@ -317,10 +398,149 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 24, "id": "d896ecee", "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089\n", + "\n", + "l1\n", + "\n", + "\n", + "\n", + "factor3\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089--factor3\n", + "\n", + "\n", + "\n", + "\n", + "factor4\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217089--factor4\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217090\n", + "\n", + "l2\n", + "\n", + "\n", + "\n", + "factor5\n", + "\n", + "\n", + "\n", + "\n", + "var7782220156096217090--factor5\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321\n", + "\n", + "x1\n", + "\n", + "\n", + "\n", + "factor0\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor0\n", + "\n", + "\n", + "\n", + "\n", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor1\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352321--factor3\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322\n", + "\n", + "x2\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor1\n", + "\n", + "\n", + "\n", + "\n", + "factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352322--factor4\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352323\n", + "\n", + "x3\n", + "\n", + "\n", + "\n", + "var8646911284551352323--factor2\n", + "\n", + "\n", + "\n", + "\n", + "var8646911284551352323--factor5\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], "source": [ "display(graphviz.Source(graph.dot(initial_estimate)))" ] @@ -343,10 +563,42 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 25, "id": "2ee6b17a", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Final Result:\n", + "Values with 5 values:\n", + "Value l1: (Eigen::Matrix)\n", + "[\n", + "\t2;\n", + "\t2\n", + "]\n", + "\n", + "Value l2: (Eigen::Matrix)\n", + "[\n", + "\t4;\n", + "\t2\n", + "]\n", + "\n", + "Value x1: (gtsam::Pose2)\n", + "(-5.72151617e-16, -2.6221043e-16, -8.93525825e-17)\n", + "\n", + "Value x2: (gtsam::Pose2)\n", + "(2, -5.76036948e-15, -6.89367166e-16)\n", + "\n", + "Value x3: (gtsam::Pose2)\n", + "(4, -1.0618198e-14, -6.48560093e-16)\n", + "\n", + "\n" + ] + } + ], "source": [ "# Optimize using Levenberg-Marquardt optimization.\n", "# The optimizer accepts optional parameters, but we'll use the defaults here.\n", @@ -371,10 +623,21 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 26, "id": "d827195e", "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], "source": [ "fig = plt.figure(1)\n", "axes = fig.add_subplot()\n", @@ -409,10 +672,40 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 27, "id": "90ef96ff", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "X1 covariance:\n", + "[[ 9.00000000e-02 4.08945493e-33 -3.19744231e-18]\n", + " [ 4.08945493e-33 9.00000000e-02 -1.27897692e-17]\n", + " [-3.19744231e-18 -1.27897692e-17 1.00000000e-02]]\n", + "\n", + "X2 covariance:\n", + "[[ 0.12096774 -0.00129032 0.00451613]\n", + " [-0.00129032 0.1583871 0.02064516]\n", + " [ 0.00451613 0.02064516 0.01774194]]\n", + "\n", + "X3 covariance:\n", + "[[0.16096774 0.00774194 0.00451613]\n", + " [0.00774194 0.35193548 0.05612903]\n", + " [0.00451613 0.05612903 0.02774194]]\n", + "\n", + "L1 covariance:\n", + "[[ 0.16870968 -0.04774194]\n", + " [-0.04774194 0.16354839]]\n", + "\n", + "L2 covariance:\n", + "[[ 0.29387097 -0.10451613]\n", + " [-0.10451613 0.39193548]]\n", + "\n" + ] + } + ], "source": [ "# Calculate and print marginal covariances for all variables.\n", "# This provides information about the uncertainty of the estimates.\n", @@ -439,10 +732,21 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 28, "id": "d1f03fee", "metadata": {}, - "outputs": [], + "outputs": [ + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], "source": [ "fig = plt.figure(2)\n", "axes = fig.add_subplot()\n", diff --git a/python/gtsam/examples/Pose2SLAMExample.ipynb b/python/gtsam/examples/Pose2SLAMExample.ipynb new file mode 100644 index 000000000..58326d371 --- /dev/null +++ b/python/gtsam/examples/Pose2SLAMExample.ipynb @@ -0,0 +1,712 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "8be9131e", + "metadata": {}, + "source": [ + "# Pose2 SLAM Example" + ] + }, + { + "cell_type": "markdown", + "id": "copyright-cell", + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "source": [ + "GTSAM Copyright 2010-2018, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information\n", + "\n", + "Simple Pose-SLAM example using only odometry measurements\n", + "Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python)" + ] + }, + { + "cell_type": "markdown", + "id": "colab-button-cell", + "metadata": { + "tags": [ + "remove-input" + ] + }, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "markdown", + "id": "intro-markdown", + "metadata": {}, + "source": [ + "This notebook demonstrates a simple 2D Simultaneous Localization and Mapping (SLAM) problem involving only robot poses and odometry measurements. A key aspect of this example is the inclusion of a **loop closure** constraint.\n", + "\n", + "**Problem Setup:**\n", + "Imagine a robot moving in a 2D plane. It receives measurements of its own motion (odometry) between consecutive time steps. Odometry is notoriously prone to drift – small errors accumulate over time, causing the robot's estimated position to diverge from its true position.\n", + "\n", + "**Loop Closure:**\n", + "A loop closure occurs when the robot recognizes a previously visited location. This provides a powerful constraint that links a later pose back to an earlier pose, significantly reducing accumulated drift. In this example, we simulate a loop closure by adding a factor that directly connects the last pose ($x_5$) back to an earlier pose ($x_2$).\n", + "\n", + "**Factor Graph:**\n", + "We will build a factor graph representing:\n", + "1. **Variables:** The unknown robot poses ($x_1, x_2, x_3, x_4, x_5$) at different time steps.\n", + "2. **Factors:**\n", + " * A **Prior Factor** on the first pose ($x_1$), anchoring the map.\n", + " * **Odometry Factors** (Between Factors) connecting consecutive poses ($x_1 \to x_2$, $x_2 \to x_3$, etc.), representing the noisy relative motion measurements.\n", + " * A **Loop Closure Factor** (also a Between Factor) connecting the last pose ($x_5$) to an earlier pose ($x_2$), representing the constraint that the robot has returned to a known location.\n", + "\n", + "We will then use GTSAM to optimize this factor graph and find the most likely sequence of robot poses given the measurements and the loop closure." + ] + }, + { + "cell_type": "markdown", + "id": "setup-imports-markdown", + "metadata": {}, + "source": [ + "## 1. Setup and Imports\n", + "\n", + "First, we install GTSAM if needed (e.g., in Google Colab) and import the necessary libraries: `gtsam`, `math` for PI, `matplotlib` for plotting, and `gtsam.utils.plot` for GTSAM-specific plotting functions." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "install-code", + "metadata": {}, + "outputs": [], + "source": [ + "# Install GTSAM from pip if running in Google Colab\n", + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not in Colab" + ] + }, + { + "cell_type": "code", + "execution_count": 15, + "id": "imports-code", + "metadata": {}, + "outputs": [], + "source": [ + "import math\n", + "import graphviz\n", + "import numpy as np\n", + "\n", + "import gtsam\n", + "import gtsam.utils.plot as gtsam_plot\n", + "import matplotlib.pyplot as plt" + ] + }, + { + "cell_type": "markdown", + "id": "noise-models-markdown", + "metadata": {}, + "source": [ + "## 2. Define Noise Models\n", + "\n", + "We define Gaussian noise models for our factors:\n", + "\n", + "* **Prior Noise:** Uncertainty on the initial pose ($x_1$). We assume the robot starts at the origin (0, 0, 0), but with some uncertainty.\n", + "* **Odometry Noise:** Uncertainty on the relative motion measurements between poses. This applies to both the sequential odometry factors and the loop closure factor." + ] + }, + { + "cell_type": "code", + "execution_count": 3, + "id": "noise-models-code", + "metadata": {}, + "outputs": [], + "source": [ + "# Create noise models with specified standard deviations (sigmas).\n", + "# For Pose2, the noise is on (x, y, theta).\n", + "# Note: gtsam.Point3 is used here to represent the 3 sigmas (dx, dy, dtheta)\n", + "\n", + "# Prior noise on the first pose (x, y, theta) - sigmas = [0.3m, 0.3m, 0.1rad]\n", + "PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))\n", + "# Odometry noise (dx, dy, dtheta) - sigmas = [0.2m, 0.2m, 0.1rad]\n", + "ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))" + ] + }, + { + "cell_type": "markdown", + "id": "build-graph-markdown", + "metadata": {}, + "source": [ + "## 3. Build the Factor Graph\n", + "\n", + "Now, we create the factor graph. We'll use simple integer keys (1, 2, 3, 4, 5) to represent the poses $x_1$ through $x_5$." + ] + }, + { + "cell_type": "code", + "execution_count": 4, + "id": "create-graph-code", + "metadata": {}, + "outputs": [], + "source": [ + "# 1. Create a factor graph container\n", + "graph = gtsam.NonlinearFactorGraph()" + ] + }, + { + "cell_type": "markdown", + "id": "add-prior-markdown", + "metadata": {}, + "source": [ + "### 3.1 Add Prior Factor\n", + "\n", + "Add a `PriorFactorPose2` on the first pose (key `1`), setting it to the origin `gtsam.Pose2(0, 0, 0)` with the defined `PRIOR_NOISE`." + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "add-prior-code", + "metadata": {}, + "outputs": [], + "source": [ + "# 2a. Add a prior on the first pose (key 1)\n", + "graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))" + ] + }, + { + "cell_type": "markdown", + "id": "add-odometry-markdown", + "metadata": {}, + "source": [ + "### 3.2 Add Odometry Factors\n", + "\n", + "Add `BetweenFactorPose2` factors for the sequential movements:\n", + "* $x_1 \to x_2$: Move 2m forward.\n", + "* $x_2 \to x_3$: Move 2m forward, turn 90 degrees left ($\\pi/2$).\n", + "* $x_3 \to x_4$: Move 2m forward, turn 90 degrees left ($\\pi/2$).\n", + "* $x_4 \to x_5$: Move 2m forward, turn 90 degrees left ($\\pi/2$).\n", + "\n", + "Each factor uses the `ODOMETRY_NOISE` model." + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "add-odometry-code", + "metadata": {}, + "outputs": [], + "source": [ + "# 2b. Add odometry factors (Between Factors)\n", + "# Between poses 1 and 2:\n", + "graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))\n", + "# Between poses 2 and 3:\n", + "graph.add(gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))\n", + "# Between poses 3 and 4:\n", + "graph.add(gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))\n", + "# Between poses 4 and 5:\n", + "graph.add(gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))" + ] + }, + { + "cell_type": "markdown", + "id": "add-loop-closure-markdown", + "metadata": {}, + "source": [ + "### 3.3 Add Loop Closure Factor\n", + "\n", + "This is the crucial step for correcting drift. We add a `BetweenFactorPose2` connecting the last pose ($x_5$, key `5`) back to the second pose ($x_2$, key `2`). The measurement represents the expected relative transform between pose 5 and pose 2 if the robot correctly returned to the location of $x_2$. We assume this measurement is also subject to `ODOMETRY_NOISE`.\n", + "\n", + "The relative pose `gtsam.Pose2(2, 0, math.pi / 2)` implies that pose 2 is 2m ahead and rotated by +90 degrees relative to pose 5." + ] + }, + { + "cell_type": "code", + "execution_count": 7, + "id": "add-loop-closure-code", + "metadata": {}, + "outputs": [], + "source": [ + "# 2c. Add the loop closure constraint\n", + "# This factor connects pose 5 back to pose 2\n", + "# The measurement is the expected relative pose from 5 to 2\n", + "graph.add(gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2), ODOMETRY_NOISE))" + ] + }, + { + "cell_type": "markdown", + "id": "inspect-graph-markdown", + "metadata": {}, + "source": [ + "### 3.4 Inspect the Graph\n", + "\n", + "Print the graph to see the factors and the variables they connect." + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "id": "inspect-graph-code", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Factor Graph:\n", + "NonlinearFactorGraph: size: 6\n", + "\n", + "Factor 0: PriorFactor on 1\n", + " prior mean: (0, 0, 0)\n", + " noise model: diagonal sigmas [0.3; 0.3; 0.1];\n", + "\n", + "Factor 1: BetweenFactor(1,2)\n", + " measured: (2, 0, 0)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 2: BetweenFactor(2,3)\n", + " measured: (2, 0, 1.57079633)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 3: BetweenFactor(3,4)\n", + " measured: (2, 0, 1.57079633)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 4: BetweenFactor(4,5)\n", + " measured: (2, 0, 1.57079633)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "Factor 5: BetweenFactor(5,2)\n", + " measured: (2, 0, 1.57079633)\n", + " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "\n", + "\n" + ] + } + ], + "source": [ + "print(\"\\nFactor Graph:\\n{}\".format(graph))" + ] + }, + { + "cell_type": "markdown", + "id": "initial-estimate-markdown", + "metadata": {}, + "source": [ + "## 4. Create Initial Estimate\n", + "\n", + "We need an initial guess for the optimizer. To illustrate the optimizer's power, we provide deliberately incorrect initial values for the poses in a `gtsam.Values` container. Without the loop closure, these errors would likely accumulate significantly." + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "id": "initial-estimate-code", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Initial Estimate:\n", + "Values with 5 values:\n", + "Value 1: (gtsam::Pose2)\n", + "(0.5, 0, 0.2)\n", + "\n", + "Value 2: (gtsam::Pose2)\n", + "(2.3, 0.1, -0.2)\n", + "\n", + "Value 3: (gtsam::Pose2)\n", + "(4.1, 0.1, 1.57079633)\n", + "\n", + "Value 4: (gtsam::Pose2)\n", + "(4, 2, 3.14159265)\n", + "\n", + "Value 5: (gtsam::Pose2)\n", + "(2.1, 2.1, -1.57079633)\n", + "\n", + "\n" + ] + } + ], + "source": [ + "# 3. Create the initial estimate for the solution\n", + "# These values are deliberately incorrect to show optimization.\n", + "initial_estimate = gtsam.Values()\n", + "initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))\n", + "initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))\n", + "initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2))\n", + "initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi))\n", + "initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2))\n", + "\n", + "print(\"\\nInitial Estimate:\\n{}\".format(initial_estimate))" + ] + }, + { + "cell_type": "markdown", + "id": "4d85a286", + "metadata": {}, + "source": [ + "Now that we have an initial estimate we can also visualize the graph:" + ] + }, + { + "cell_type": "code", + "execution_count": 17, + "id": "40471c87", + "metadata": {}, + "outputs": [ + { + "data": { + "image/svg+xml": [ + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "\n", + "var1\n", + "\n", + "1\n", + "\n", + "\n", + "\n", + "factor0\n", + "\n", + "\n", + "\n", + "\n", + "var1--factor0\n", + "\n", + "\n", + "\n", + "\n", + "factor1\n", + "\n", + "\n", + "\n", + "\n", + "var1--factor1\n", + "\n", + "\n", + "\n", + "\n", + "var2\n", + "\n", + "2\n", + "\n", + "\n", + "\n", + "var2--factor1\n", + "\n", + "\n", + "\n", + "\n", + "factor2\n", + "\n", + "\n", + "\n", + "\n", + "var2--factor2\n", + "\n", + "\n", + "\n", + "\n", + "factor5\n", + "\n", + "\n", + "\n", + "\n", + "var2--factor5\n", + "\n", + "\n", + "\n", + "\n", + "var3\n", + "\n", + "3\n", + "\n", + "\n", + "\n", + "var3--factor2\n", + "\n", + "\n", + "\n", + "\n", + "factor3\n", + "\n", + "\n", + "\n", + "\n", + "var3--factor3\n", + "\n", + "\n", + "\n", + "\n", + "var4\n", + "\n", + "4\n", + "\n", + "\n", + "\n", + "var4--factor3\n", + "\n", + "\n", + "\n", + "\n", + "factor4\n", + "\n", + "\n", + "\n", + "\n", + "var4--factor4\n", + "\n", + "\n", + "\n", + "\n", + "var5\n", + "\n", + "5\n", + "\n", + "\n", + "\n", + "var5--factor4\n", + "\n", + "\n", + "\n", + "\n", + "var5--factor5\n", + "\n", + "\n", + "\n", + "\n" + ], + "text/plain": [ + "" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "display(graphviz.Source(graph.dot(initial_estimate), engine='neato'))" + ] + }, + { + "cell_type": "markdown", + "id": "optimize-markdown", + "metadata": {}, + "source": [ + "## 5. Optimize the Factor Graph\n", + "\n", + "We'll use the Gauss-Newton optimizer to find the most likely configuration of poses.\n", + "\n", + "1. Set optimization parameters using `gtsam.GaussNewtonParams` (e.g., error tolerance, max iterations).\n", + "2. Create the `gtsam.GaussNewtonOptimizer` instance with the graph, initial estimate, and parameters.\n", + "3. Run `optimizer.optimize()`." + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "id": "optimize-code", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Final Result:\n", + "Values with 5 values:\n", + "Value 1: (gtsam::Pose2)\n", + "(-8.50051783e-21, -7.35289215e-20, -2.34289062e-20)\n", + "\n", + "Value 2: (gtsam::Pose2)\n", + "(2, -1.53066255e-19, -3.05180521e-20)\n", + "\n", + "Value 3: (gtsam::Pose2)\n", + "(4, -3.42173677e-11, 1.57079633)\n", + "\n", + "Value 4: (gtsam::Pose2)\n", + "(4, 2, 3.14159265)\n", + "\n", + "Value 5: (gtsam::Pose2)\n", + "(2, 2, -1.57079633)\n", + "\n", + "\n" + ] + } + ], + "source": [ + "# 4. Optimize the initial values using Gauss-Newton\n", + "parameters = gtsam.GaussNewtonParams()\n", + "\n", + "# Set optimization parameters\n", + "parameters.setRelativeErrorTol(1e-5) # Stop when change in error is small\n", + "parameters.setMaxIterations(100) # Limit iterations\n", + "\n", + "# Create the optimizer\n", + "optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters)\n", + "\n", + "# Optimize!\n", + "result = optimizer.optimize()\n", + "\n", + "print(\"\\nFinal Result:\\n{}\".format(result))" + ] + }, + { + "cell_type": "markdown", + "id": "marginals-markdown", + "metadata": {}, + "source": [ + "## 6. Calculate Marginal Covariances\n", + "\n", + "After optimization, we can compute the uncertainty (covariance) associated with each estimated pose using `gtsam.Marginals`." + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "id": "marginals-code", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "Marginal Covariances:\n", + "X1 covariance:\n", + "[[ 9.00000000e-02 1.96306337e-18 -1.49103687e-17]\n", + " [ 1.96306337e-18 9.00000000e-02 -7.03437308e-17]\n", + " [-1.49103687e-17 -7.03437308e-17 1.00000000e-02]]\n", + "\n", + "X2 covariance:\n", + "[[ 1.30000000e-01 -3.89782542e-17 -4.37043325e-17]\n", + " [-3.89782542e-17 1.70000000e-01 2.00000000e-02]\n", + " [-4.37043325e-17 2.00000000e-02 2.00000000e-02]]\n", + "\n", + "X3 covariance:\n", + "[[ 3.62000000e-01 -3.29291732e-12 6.20000000e-02]\n", + " [-3.29291394e-12 1.62000000e-01 -2.00000000e-03]\n", + " [ 6.20000000e-02 -2.00000000e-03 2.65000000e-02]]\n", + "\n", + "X4 covariance:\n", + "[[ 0.268 -0.128 0.048]\n", + " [-0.128 0.378 -0.068]\n", + " [ 0.048 -0.068 0.028]]\n", + "\n", + "X5 covariance:\n", + "[[ 0.202 0.036 -0.018 ]\n", + " [ 0.036 0.26 -0.051 ]\n", + " [-0.018 -0.051 0.0265]]\n", + "\n" + ] + } + ], + "source": [ + "# 5. Calculate and print marginal covariances\n", + "marginals = gtsam.Marginals(graph, result)\n", + "print(\"\\nMarginal Covariances:\")\n", + "for i in range(1, 6):\n", + " print(f\"X{i} covariance:\\n{marginals.marginalCovariance(i)}\\n\")" + ] + }, + { + "cell_type": "markdown", + "id": "visualize-markdown", + "metadata": {}, + "source": [ + "## 7. Visualize Results\n", + "\n", + "Finally, we use `gtsam.utils.plot.plot_pose2` to visualize the optimized poses along with their covariance ellipses. Notice how the poses form a square, and the loop closure (connecting pose 5 back to pose 2) helps maintain this structure despite the initial errors and odometry noise. The covariance ellipses show the uncertainty, which is typically smallest at the prior (pose 1) and might be reduced near the loop closure." + ] + }, + { + "cell_type": "code", + "execution_count": 13, + "id": "visualize-code", + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "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", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ + "# Plot the results\n", + "fig = plt.figure(0, figsize=(8, 8))\n", + "\n", + "for i in range(1, 6):\n", + " # Plot pose with covariance ellipse\n", + " gtsam_plot.plot_pose2(fig.number, result.atPose2(i), axis_length=0.4,\n", + " covariance=marginals.marginalCovariance(i))\n", + "\n", + "# Adjust plot settings\n", + "plt.title(\"Optimized Poses with Covariance Ellipses\")\n", + "plt.xlabel(\"X axis\")\n", + "plt.ylabel(\"Y axis\")\n", + "plt.axis('equal') # Ensure equal scaling on x and y axes\n", + "plt.show() # Display the plot" + ] + }, + { + "cell_type": "markdown", + "id": "summary-markdown", + "metadata": {}, + "source": [ + "## Summary\n", + "\n", + "This example demonstrated a Pose SLAM problem where:\n", + "1. We modeled robot poses and odometry measurements using a `gtsam.NonlinearFactorGraph`.\n", + "2. A prior was added to the first pose.\n", + "3. Sequential odometry factors were added between consecutive poses.\n", + "4. A crucial loop closure factor was added, connecting the last pose back to an earlier one.\n", + "5. An inaccurate initial estimate was provided.\n", + "6. The `gtsam.GaussNewtonOptimizer` was used to find the optimal pose estimates.\n", + "7. Marginal covariances were calculated to show the uncertainty.\n", + "8. The results, including covariance ellipses, were visualized, highlighting the effect of the loop closure in correcting drift." + ] + } + ], + "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 +} diff --git a/python/gtsam/examples/Pose2SLAMExample.py b/python/gtsam/examples/Pose2SLAMExample.py deleted file mode 100644 index 300a70fbd..000000000 --- a/python/gtsam/examples/Pose2SLAMExample.py +++ /dev/null @@ -1,102 +0,0 @@ -""" -GTSAM Copyright 2010-2018, Georgia Tech Research Corporation, -Atlanta, Georgia 30332-0415 -All Rights Reserved -Authors: Frank Dellaert, et al. (see THANKS for the full author list) - -See LICENSE for the license information - -Simple robotics example using odometry measurements and bearing-range (laser) measurements -Author: Alex Cunningham (C++), Kevin Deng & Frank Dellaert (Python) -""" -# pylint: disable=invalid-name, E1101 - -from __future__ import print_function - -import math - -import gtsam -import gtsam.utils.plot as gtsam_plot -import matplotlib.pyplot as plt - - -def main(): - """Main runner.""" - # Create noise models - PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(gtsam.Point3(0.3, 0.3, 0.1)) - ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas( - gtsam.Point3(0.2, 0.2, 0.1)) - - # 1. Create a factor graph container and add factors to it - graph = gtsam.NonlinearFactorGraph() - - # 2a. Add a prior on the first pose, setting it to the origin - # A prior factor consists of a mean and a noise ODOMETRY_NOISE (covariance matrix) - graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE)) - - # 2b. Add odometry factors - # Create odometry (Between) factors between consecutive poses - graph.add( - gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE)) - graph.add( - gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, math.pi / 2), - ODOMETRY_NOISE)) - graph.add( - gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2, 0, math.pi / 2), - ODOMETRY_NOISE)) - graph.add( - gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2, 0, math.pi / 2), - ODOMETRY_NOISE)) - - # 2c. Add the loop closure constraint - # This factor encodes the fact that we have returned to the same pose. In real - # systems, these constraints may be identified in many ways, such as appearance-based - # techniques with camera images. We will use another Between Factor to enforce this constraint: - graph.add( - gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2, 0, math.pi / 2), - ODOMETRY_NOISE)) - print("\nFactor Graph:\n{}".format(graph)) # print - - # 3. Create the data structure to hold the initial_estimate estimate to the - # solution. For illustrative purposes, these have been deliberately set to incorrect values - initial_estimate = gtsam.Values() - initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2)) - initial_estimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2)) - initial_estimate.insert(3, gtsam.Pose2(4.1, 0.1, math.pi / 2)) - initial_estimate.insert(4, gtsam.Pose2(4.0, 2.0, math.pi)) - initial_estimate.insert(5, gtsam.Pose2(2.1, 2.1, -math.pi / 2)) - print("\nInitial Estimate:\n{}".format(initial_estimate)) # print - - # 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer - # The optimizer accepts an optional set of configuration parameters, - # controlling things like convergence criteria, the type of linear - # system solver to use, and the amount of information displayed during - # optimization. We will set a few parameters as a demonstration. - parameters = gtsam.GaussNewtonParams() - - # Stop iterating once the change in error between steps is less than this value - parameters.setRelativeErrorTol(1e-5) - # Do not perform more than N iteration steps - parameters.setMaxIterations(100) - # Create the optimizer ... - optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, parameters) - # ... and optimize - result = optimizer.optimize() - print("Final Result:\n{}".format(result)) - - # 5. Calculate and print marginal covariances for all variables - marginals = gtsam.Marginals(graph, result) - for i in range(1, 6): - print("X{} covariance:\n{}\n".format(i, - marginals.marginalCovariance(i))) - - for i in range(1, 6): - gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5, - marginals.marginalCovariance(i)) - - plt.axis('equal') - plt.show() - - -if __name__ == "__main__": - main() From 104bba242225d86d18f7f1bf6e8ec9050c2acdda Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Wed, 23 Apr 2025 08:54:38 -0400 Subject: [PATCH 3/9] EKF-SLAM proto --- python/gtsam/examples/EKF_SLAM.ipynb | 619 +++++++++++++++++++++++++++ 1 file changed, 619 insertions(+) create mode 100644 python/gtsam/examples/EKF_SLAM.ipynb diff --git a/python/gtsam/examples/EKF_SLAM.ipynb b/python/gtsam/examples/EKF_SLAM.ipynb new file mode 100644 index 000000000..9422204f1 --- /dev/null +++ b/python/gtsam/examples/EKF_SLAM.ipynb @@ -0,0 +1,619 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "27cd96fc", + "metadata": {}, + "source": [ + "# EKF-SLAM using Factor Graphs (SRIF-like)" + ] + }, + { + "cell_type": "markdown", + "id": "copyright-cell-srif", + "metadata": { + "tags": [ + "remove-cell" + ] + }, + "source": [ + "GTSAM Copyright 2010-2023, Georgia Tech Research Corporation,\n", + "Atlanta, Georgia 30332-0415\n", + "All Rights Reserved\n", + "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n", + "\n", + "See LICENSE for the license information" + ] + }, + { + "cell_type": "markdown", + "id": "colab-button-cell-srif", + "metadata": {}, + "source": [ + "\"Open" + ] + }, + { + "cell_type": "markdown", + "id": "intro-srif", + "metadata": {}, + "source": [ + "This notebook demonstrates 2D SLAM using GTSAM's factor graph tools in an **iterative, filter-like manner**, similar in structure to a Square Root Information Filter (SRIF) or an EKF, but leveraging factor graph elimination instead of explicit matrix operations.\n", + "\n", + "**Scenario:**\n", + "A robot moves in a circular path within a 10x10 meter area containing randomly placed landmarks. It receives noisy odometry and bearing-range measurements.\n", + "\n", + "**Factor Graph Filtering Approach:**\n", + "Instead of maintaining a large state vector and covariance matrix explicitly (like in classic EKF), we use GTSAM's `NonlinearFactorGraph` and `GaussianFactorGraph`:\n", + "\n", + "1. **Belief Representation:** The robot's belief (estimated state and uncertainty) after each step is implicitly represented by a factor graph (or the resulting Bayes Net after elimination).\n", + "2. **Prediction:**\n", + " * Start with the factors representing the belief from the *previous* step.\n", + " * Add a new `BetweenFactorPose2` representing the noisy odometry measurement, connecting the previous pose `X(k)` to the new pose `X(k+1)`.\n", + " * Linearize all relevant factors around the current best estimate (`Values`).\n", + " * Eliminate the resulting `GaussianFactorGraph` using an ordering that marginalizes out the previous pose `X(k)`, yielding a `GaussianBayesNet` representing the *predictive* distribution over landmarks and `X(k+1)`.\n", + "3. **Update:**\n", + " * Start with the factors representing the *predictive* belief.\n", + " * For each landmark measurement at the new pose `X(k+1)`:\n", + " * If the landmark `L(j)` is new, add it to the `Values` (initial estimate).\n", + " * Add a `BearingRangeFactor2D` connecting `X(k+1)` and `L(j)`.\n", + " * Linearize new/updated factors.\n", + " * Eliminate the `GaussianFactorGraph` to incorporate the measurement information, yielding a `GaussianBayesNet` representing the *posterior* belief.\n", + " * Optimize the final Bayes Net to get the updated state estimate (`Values`).\n", + "4. **Iteration:** Repeat prediction and update for each time step.\n", + "\n", + "**Advantages:**\n", + "* Leverages GTSAM's robust factor graph machinery.\n", + "* Handles non-linearities through iterative linearization (like EKF, but potentially more robust within GTSAM's optimization context).\n", + "* Avoids explicit management of large, dense covariance matrices.\n", + "\n", + "**Output:** The process will be visualized using a Plotly animation showing the robot's estimated path, mapped landmarks, and uncertainty ellipses evolving over time." + ] + }, + { + "cell_type": "markdown", + "id": "setup-imports-srif", + "metadata": {}, + "source": [ + "## 1. Setup and Imports" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "install-code-srif", + "metadata": {}, + "outputs": [], + "source": [ + "# Install GTSAM and Plotly from pip if running in Google Colab\n", + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop plotly\n", + "except ImportError:\n", + " pass # Not in Colab" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "imports-code-srif", + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "import matplotlib.pyplot as plt # For initial plot if desired\n", + "import plotly.graph_objects as go\n", + "from tqdm.notebook import tqdm # Progress bar\n", + "import math\n", + "\n", + "import gtsam\n", + "from gtsam.symbol_shorthand import X, L # Symbols for poses and landmarks" + ] + }, + { + "cell_type": "markdown", + "id": "params-srif", + "metadata": {}, + "source": [ + "## 2. Simulation Parameters" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "params-code-srif", + "metadata": {}, + "outputs": [], + "source": [ + "# World parameters\n", + "NUM_LANDMARKS = 15\n", + "WORLD_SIZE = 10.0 # Environment bounds [-WORLD_SIZE/2, WORLD_SIZE/2]\n", + "\n", + "# Robot parameters\n", + "ROBOT_RADIUS = 3.0\n", + "ROBOT_ANGULAR_VEL = np.deg2rad(20.0) # Radians per step\n", + "NUM_STEPS = 50 # Reduced steps for faster animation generation\n", + "DT = 1.0 # Time step duration (simplified)\n", + "\n", + "# Noise parameters (GTSAM Noise Models)\n", + "# Prior noise on the first pose (x, y, theta)\n", + "PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, np.deg2rad(1.0)]))\n", + "# Odometry noise (dx, dy, dtheta)\n", + "ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.05, np.deg2rad(2.0)]))\n", + "# Measurement noise (bearing, range)\n", + "MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([np.deg2rad(2.0), 0.2]))\n", + "\n", + "# Create samplers for noise models\n", + "prior_noise_sampler = gtsam.Sampler(PRIOR_NOISE, 42)\n", + "odometry_noise_sampler = gtsam.Sampler(ODOMETRY_NOISE, 42)\n", + "measurement_noise_sampler = gtsam.Sampler(MEASUREMENT_NOISE, 42)\n", + "\n", + "# Sensor parameters\n", + "MAX_SENSOR_RANGE = 5.0" + ] + }, + { + "cell_type": "markdown", + "id": "ground-truth-srif", + "metadata": {}, + "source": [ + "## 3. Generate Ground Truth Data\n", + "\n", + "Create true landmark positions, robot path, and simulate noisy measurements." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "ground-truth-code-srif", + "metadata": {}, + "outputs": [], + "source": [ + "# 3.1 Ground Truth Landmarks\n", + "landmarks_gt = (np.random.rand(2, NUM_LANDMARKS) - 0.5) * WORLD_SIZE\n", + "landmark_ids = list(range(NUM_LANDMARKS))\n", + "landmarks_gt_dict = {L(i): gtsam.Point2(landmarks_gt[:, i]) for i in range(NUM_LANDMARKS)}\n", + "\n", + "# 3.2 Ground Truth Robot Path\n", + "poses_gt = []\n", + "current_pose_gt = gtsam.Pose2(ROBOT_RADIUS, 0, np.pi / 2) # Start on circle edge\n", + "poses_gt.append(current_pose_gt)\n", + "\n", + "for k in range(NUM_STEPS):\n", + " delta_theta = ROBOT_ANGULAR_VEL * DT\n", + " arc_length = ROBOT_ANGULAR_VEL * ROBOT_RADIUS * DT\n", + " motion_command = gtsam.Pose2(arc_length, 0, delta_theta)\n", + " current_pose_gt = current_pose_gt.compose(motion_command)\n", + " poses_gt.append(current_pose_gt)\n", + "\n", + "# 3.3 Simulate Noisy Odometry Measurements (as Pose2 objects)\n", + "odometry_measurements = []\n", + "for k in range(NUM_STEPS):\n", + " pose_k = poses_gt[k]\n", + " pose_k1 = poses_gt[k+1]\n", + " true_odom = pose_k.between(pose_k1)\n", + " \n", + " # Sample noise directly for Pose2 composition (approximate)\n", + " # A more rigorous approach samples from the tangent space\n", + " odom_noise_vec = odometry_noise_sampler.sample()\n", + " noisy_odom = true_odom.compose(gtsam.Pose2(odom_noise_vec[0], odom_noise_vec[1], odom_noise_vec[2]))\n", + " odometry_measurements.append(noisy_odom)\n", + "\n", + "# 3.4 Simulate Noisy Bearing-Range Measurements\n", + "# measurements_sim[k] = list of (L(lm_id), range, bearing) for measurements *at* pose k\n", + "measurements_sim = [[] for _ in range(NUM_STEPS + 1)]\n", + "for k in range(NUM_STEPS + 1):\n", + " robot_pose = poses_gt[k]\n", + " for lm_id in range(NUM_LANDMARKS):\n", + " lm_gt_pt = landmarks_gt_dict[L(lm_id)]\n", + " try:\n", + " # Use BearingRangeFactor2D to simulate the measurement model\n", + " measurement_factor = gtsam.BearingRangeFactor2D(X(k), L(lm_id), \n", + " robot_pose.bearing(lm_gt_pt),\n", + " robot_pose.range(lm_gt_pt), \n", + " MEASUREMENT_NOISE)\n", + " true_range = measurement_factor.measured().range()\n", + " true_bearing = measurement_factor.measured().bearing()\n", + "\n", + " if true_range <= MAX_SENSOR_RANGE and abs(true_bearing.theta()) 0: # Ensure range is positive\n", + " measurements_sim[k].append((L(lm_id), noisy_bearing, noisy_range))\n", + " except Exception as e:\n", + " # Catch potential errors like point being too close to the pose\n", + " print(f\"Sim Warning at step {k}, landmark {lm_id}: {e}\")\n", + " pass\n", + "\n", + "print(f\"Generated {NUM_LANDMARKS} landmarks.\")\n", + "print(f\"Generated {NUM_STEPS} ground truth poses and odometry measurements.\")\n", + "num_meas_total = sum(len(m_list) for m_list in measurements_sim)\n", + "print(f\"Generated {num_meas_total} bearing-range measurements across all steps.\")" + ] + }, + { + "cell_type": "markdown", + "id": "factor-graph-slam-impl", + "metadata": {}, + "source": [ + "## 4. Iterative Factor Graph SLAM Implementation\n", + "\n", + "Here we perform the step-by-step prediction and update using factor graphs." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "factor-graph-init-code", + "metadata": {}, + "outputs": [], + "source": [ + "# --- Initialization ---\n", + "current_estimate = gtsam.Values() # Stores current best estimates (linearization points)\n", + "current_graph = gtsam.NonlinearFactorGraph() # Stores factors added so far\n", + "current_pose_key = X(0)\n", + "\n", + "# Add prior on the initial pose X(0)\n", + "initial_pose = poses_gt[0] # Start at ground truth (or add noise)\n", + "current_estimate.insert(current_pose_key, initial_pose)\n", + "current_graph.add(gtsam.PriorFactorPose2(current_pose_key, initial_pose, PRIOR_NOISE))\n", + "\n", + "# Variables to store results for animation\n", + "results_history = [current_estimate] # Store Values object at each step\n", + "marginals_history = [] # Store Marginals object at each step\n", + "known_landmarks = set() # Set of landmark keys L(j) currently in the state\n", + "\n", + "# Initial marginals (only for X(0))\n", + "try:\n", + " initial_gaussian_graph = current_graph.linearize(current_estimate)\n", + " initial_marginals = gtsam.Marginals(initial_gaussian_graph, current_estimate)\n", + " marginals_history.append(initial_marginals)\n", + "except Exception as e:\n", + " print(f\"Error computing initial marginals: {e}\")\n", + " marginals_history.append(None) # Append placeholder if fails\n" + ] + }, + { + "cell_type": "markdown", + "id": "factor-graph-loop", + "metadata": {}, + "source": [ + "### Main Iterative Loop" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "factor-graph-loop-code", + "metadata": {}, + "outputs": [], + "source": [ + "print(\"Running Iterative Factor Graph SLAM...\")\n", + "\n", + "for k in tqdm(range(NUM_STEPS)):\n", + " # === Prediction Step ===\n", + " prev_pose_key = X(k)\n", + " current_pose_key = X(k + 1)\n", + " odom_k = odometry_measurements[k]\n", + " \n", + " # Predict next pose and add to Values (linearization point)\n", + " predicted_pose = current_estimate.atPose2(prev_pose_key).compose(odom_k)\n", + " current_estimate.insert(current_pose_key, predicted_pose)\n", + " \n", + " # Add odometry factor to the graph\n", + " current_graph.add(gtsam.BetweenFactorPose2(prev_pose_key, current_pose_key, odom_k, ODOMETRY_NOISE))\n", + "\n", + " # --- At this point, the full graph contains the history --- \n", + " # --- To mimic SRIF/EKF, we *could* eliminate prev_pose_key, but --- \n", + " # --- it's simpler here to keep the full graph and optimize it --- \n", + " # --- then extract the current belief for the next step's factors --- \n", + " # --- Let's proceed with adding measurements first ----\n", + "\n", + " # === Update Step ===\n", + " measurements_k1 = measurements_sim[k + 1] # Measurements taken AT pose k+1\n", + " landmarks_observed_this_step = set()\n", + "\n", + " for lm_key, measured_bearing, measured_range in measurements_k1:\n", + " landmarks_observed_this_step.add(lm_key)\n", + " \n", + " # If landmark is new, initialize its estimate\n", + " if not current_estimate.exists(lm_key):\n", + " # Initial guess based on current pose estimate and measurement\n", + " current_pose_val = current_estimate.atPose2(current_pose_key)\n", + " delta_x = measured_range * math.cos(measured_bearing.theta())\n", + " delta_y = measured_range * math.sin(measured_bearing.theta())\n", + " # Transform delta from robot frame to world frame\n", + " lm_initial_guess = current_pose_val.transformFrom(gtsam.Point2(delta_x, delta_y))\n", + " current_estimate.insert(lm_key, lm_initial_guess)\n", + " known_landmarks.add(lm_key)\n", + " # print(f\"Step {k+1}: Initialized Landmark {lm_key.index()}\")\n", + "\n", + " # Add measurement factor\n", + " current_graph.add(gtsam.BearingRangeFactor2D(current_pose_key, lm_key, \n", + " measured_bearing, measured_range,\n", + " MEASUREMENT_NOISE))\n", + " \n", + " # --- Optimization (Incremental Smoothing) ---\n", + " # In a true filter, we'd eliminate previous states.\n", + " # Here, we re-optimize the growing graph. For efficiency in large problems, \n", + " # iSAM2 would be used, but Levenberg-Marquardt on the growing graph \n", + " # demonstrates the concept for this smaller example.\n", + " \n", + " optimizer = gtsam.LevenbergMarquardtOptimizer(current_graph, current_estimate)\n", + " try:\n", + " current_estimate = optimizer.optimize() # Update estimates by optimizing the whole graph so far\n", + " except Exception as e:\n", + " print(f\"Optimization failed at step {k+1}: {e}\")\n", + " # Potentially revert estimate update or handle error\n", + " # For simplicity, we continue with the potentially unoptimized estimate\n", + " pass \n", + "\n", + " # Store results for animation\n", + " results_history.append(gtsam.Values(current_estimate)) # Store a copy\n", + " \n", + " # Calculate marginals for visualization (can be slow for large graphs)\n", + " try:\n", + " current_gaussian_graph = current_graph.linearize(current_estimate)\n", + " current_marginals = gtsam.Marginals(current_gaussian_graph, current_estimate)\n", + " marginals_history.append(current_marginals)\n", + " except Exception as e:\n", + " print(f\"Marginals calculation failed at step {k+1}: {e}\")\n", + " marginals_history.append(None) # Append placeholder\n", + "\n", + "print(\"Iterative Factor Graph SLAM finished.\")\n", + "print(f\"Final number of poses: {k+2}\")\n", + "print(f\"Number of landmarks mapped: {len(known_landmarks)}\")" + ] + }, + { + "cell_type": "markdown", + "id": "plotly-animation", + "metadata": {}, + "source": [ + "## 5. Create Plotly Animation" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "plotly-animation-code", + "metadata": { + "scrolled": false, + "tags": [ + "remove-input" + ] + }, + "outputs": [], + "source": [ + "print(\"Generating Plotly animation...\")\n", + "\n", + "def ellipse_path(cx, cy, sizex, sizey, angle, N=60):\n", + " \"\"\"SVG path string for an ellipse centered at (cx, cy), rotated by `angle` in degrees.\"\"\"\n", + " angle_rad = np.radians(angle)\n", + " t = np.linspace(0, 2 * np.pi, N)\n", + " x = (sizex / 2) * np.cos(t)\n", + " y = (sizey / 2) * np.sin(t)\n", + "\n", + " x_rot = cx + x * np.cos(angle_rad) - y * np.sin(angle_rad)\n", + " y_rot = cy + x * np.sin(angle_rad) + y * np.cos(angle_rad)\n", + "\n", + " path = f\"M {x_rot[0]},{y_rot[0]} \" + \" \".join(\n", + " f\"L{x_},{y_}\" for x_, y_ in zip(x_rot[1:], y_rot[1:])\n", + " ) + \" Z\"\n", + " return path\n", + "\n", + "# Helper to convert GTSAM covariance to Plotly ellipse parameters\n", + "def gtsam_cov_to_plotly_ellipse(cov_matrix, scale=2.0):\n", + " \"\"\"Calculates ellipse parameters (angle, width, height) from a 2x2 covariance matrix.\"\"\"\n", + " # Ensure positive definite - add small epsilon if needed\n", + " cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 \n", + " try:\n", + " eigvals, eigvecs = np.linalg.eigh(cov)\n", + " # Ensure eigenvalues are positive for sqrt\n", + " eigvals = np.maximum(eigvals, 1e-9)\n", + " except np.linalg.LinAlgError:\n", + " print(\"Warning: Covariance matrix SVD failed, using default ellipse.\")\n", + " return 0, 0.1*scale, 0.1*scale # Default small ellipse\n", + " \n", + " # Width/Height are 2*scale*sqrt(eigenvalue)\n", + " width = 2 * scale * np.sqrt(eigvals[1])\n", + " height = 2 * scale * np.sqrt(eigvals[0])\n", + " \n", + " # Angle of the major axis (corresponding to largest eigenvalue)\n", + " angle_rad = np.arctan2(eigvecs[1, 1], eigvecs[0, 1])\n", + " angle_deg = np.degrees(angle_rad)\n", + " \n", + " return angle_deg, width, height\n", + "\n", + "# --- Create Plotly Figure --- \n", + "fig = go.Figure()\n", + "\n", + "# Add Ground Truth Landmarks (static)\n", + "fig.add_trace(go.Scatter(\n", + " x=landmarks_gt[0, :], \n", + " y=landmarks_gt[1, :], \n", + " mode='markers', \n", + " marker=dict(color='black', size=8, symbol='star'),\n", + " name='Landmarks GT'\n", + "))\n", + "\n", + "# Add Ground Truth Path (static)\n", + "gt_path_x = [p.x() for p in poses_gt]\n", + "gt_path_y = [p.y() for p in poses_gt]\n", + "fig.add_trace(go.Scatter(\n", + " x=gt_path_x,\n", + " y=gt_path_y,\n", + " mode='lines',\n", + " line=dict(color='gray', width=1, dash='dash'),\n", + " name='Path GT'\n", + "))\n", + "\n", + "# --- Animation Frames --- \n", + "frames = []\n", + "steps = list(range(NUM_STEPS + 1))\n", + "\n", + "for k in tqdm(steps):\n", + " frame_data = []\n", + " step_results = results_history[k]\n", + " step_marginals = marginals_history[k]\n", + " \n", + " # Estimated Path up to step k\n", + " est_path_x = [results_history[i].atPose2(X(i)).x() for i in range(k + 1)]\n", + " est_path_y = [results_history[i].atPose2(X(i)).y() for i in range(k + 1)]\n", + " frame_data.append(go.Scatter(\n", + " x=est_path_x,\n", + " y=est_path_y,\n", + " mode='lines+markers',\n", + " line=dict(color='red', width=2),\n", + " marker=dict(size=4, color='red'),\n", + " name='Path Est'\n", + " ))\n", + " \n", + " # Estimated Landmarks known at step k\n", + " est_landmarks_x = []\n", + " est_landmarks_y = []\n", + " landmark_keys_in_frame = []\n", + " for lm_key in step_results.keys():\n", + " if gtsam.symbolChr(lm_key)==108: # Check if it's a landmark key\n", + " lm_pose = step_results.atPoint2(lm_key)\n", + " est_landmarks_x.append(lm_pose[0])\n", + " est_landmarks_y.append(lm_pose[1])\n", + " landmark_keys_in_frame.append(lm_key)\n", + " \n", + " if est_landmarks_x:\n", + " frame_data.append(go.Scatter(\n", + " x=est_landmarks_x, \n", + " y=est_landmarks_y, \n", + " mode='markers',\n", + " marker=dict(color='blue', size=6, symbol='x'),\n", + " name='Landmarks Est'\n", + " ))\n", + "\n", + " # Current Pose Covariance Ellipse\n", + " shapes = [] # List to hold ellipse shapes for this frame\n", + " if step_marginals is not None:\n", + " try:\n", + " current_pose_key = X(k)\n", + " pose_cov = step_marginals.marginalCovariance(current_pose_key)\n", + " pose_mean = step_results.atPose2(current_pose_key).translation()\n", + " angle, width, height = gtsam_cov_to_plotly_ellipse(pose_cov)\n", + " cx, cy = pose_mean[0], pose_mean[1]\n", + " shapes.append(dict(\n", + " type=\"path\",\n", + " path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle, N=60),\n", + " xref=\"x\", yref=\"y\",\n", + " fillcolor=\"rgba(255,0,255,0.2)\",\n", + " line_color=\"rgba(255,0,255,0.5)\",\n", + " name=f'Pose {k} Cov'\n", + " ))\n", + " except Exception as e:\n", + " print(f\"Warning: Failed getting pose cov ellipse at step {k}: {e}\")\n", + "\n", + " # Landmark Covariance Ellipses\n", + " for lm_key in landmark_keys_in_frame:\n", + " index = gtsam.symbolIndex(lm_key)\n", + " try:\n", + " lm_cov = step_marginals.marginalCovariance(lm_key)\n", + " lm_mean = step_results.atPoint2(lm_key)\n", + " angle, width, height = gtsam_cov_to_plotly_ellipse(lm_cov)\n", + " cx, cy = lm_mean[0], lm_mean[1]\n", + " shapes.append(dict(\n", + " type=\"path\",\n", + " path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle, N=60),\n", + " xref=\"x\", yref=\"y\",\n", + " fillcolor=\"rgba(0,0,255,0.1)\",\n", + " line_color=\"rgba(0,0,255,0.3)\",\n", + " name=f'LM {index} Cov'\n", + " ))\n", + " except Exception as e:\n", + " print(f\"Warning: Failed getting landmark {index} cov ellipse at step {k}: {e}\")\n", + "\n", + " frames.append(go.Frame(data=frame_data, name=str(k), layout=go.Layout(shapes=shapes)))\n", + "\n", + "# --- Set Initial State and Layout --- \n", + "fig.update(frames=frames)\n", + "\n", + "# Set initial data to the first frame's data\n", + "if frames:\n", + " fig.add_traces(frames[0].data)\n", + " initial_shapes = frames[0].layout.shapes if frames[0].layout else []\n", + "else:\n", + " initial_shapes = []\n", + "\n", + "# Define slider\n", + "sliders = [dict(\n", + " active=0,\n", + " currentvalue={\"prefix\": \"Step: \"},\n", + " pad={\"t\": 50},\n", + " steps=[dict(label=str(k), \n", + " method='animate', \n", + " args=[[str(k)], \n", + " dict(mode='immediate', \n", + " frame=dict(duration=100, redraw=True), \n", + " transition=dict(duration=0))])\n", + " for k in steps]\n", + ")]\n", + "\n", + "# Update layout\n", + "fig.update_layout(\n", + " title='Iterative Factor Graph SLAM Animation',\n", + " xaxis=dict(range=[-WORLD_SIZE/2 - 2, WORLD_SIZE/2 + 2], constrain='domain'),\n", + " yaxis=dict(range=[-WORLD_SIZE/2 - 2, WORLD_SIZE/2 + 2], scaleanchor='x', scaleratio=1),\n", + " width=800, height=800,\n", + " hovermode='closest',\n", + " updatemenus=[dict(type='buttons', \n", + " showactive=False,\n", + " buttons=[dict(label='Play',\n", + " method='animate', \n", + " args=[None, \n", + " dict(mode='immediate', \n", + " frame=dict(duration=100, redraw=True), \n", + " transition=dict(duration=0), \n", + " fromcurrent=True)])])],\n", + " sliders=sliders,\n", + " shapes=initial_shapes # Set initial shapes\n", + ")\n", + "\n", + "print(\"Displaying animation...\")\n", + "fig.show()" + ] + }, + { + "cell_type": "markdown", + "id": "discussion-srif", + "metadata": {}, + "source": [ + "## 6. Discussion\n", + "\n", + "* **Approach:** This notebook implemented SLAM iteratively using GTSAM factor graphs. At each step, new factors (odometry, measurements) were added, and the graph was re-optimized using Levenberg-Marquardt. This is more akin to **incremental smoothing** than a classic filter (which would explicitly marginalize past states).\n", + "* **Efficiency:** Optimizing the entire graph at every step becomes computationally expensive for long trajectories. For real-time performance or large problems, **iSAM2 (Incremental Smoothing and Mapping)** is the preferred GTSAM algorithm. iSAM2 efficiently updates the solution by only re-linearizing and re-solving parts of the graph affected by new measurements.\n", + "* **Accuracy vs. EKF:** This factor graph approach generally handles non-linearities better than a standard EKF because it re-linearizes during optimization. It avoids some of the consistency pitfalls associated with the EKF's single linearization point per step.\n", + "* **Visualization:** The Plotly animation shows the evolution of the robot's path estimate, the map of landmarks, and their associated uncertainties (covariance ellipses). You can observe how measurements help refine the estimates and reduce uncertainty, especially when loops are closed (implicitly here through repeated observations of landmarks)." + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "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.10.12" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} From 423e447de0f559e3f3a85bc8c60b85a4ee2e0fa0 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Apr 2025 15:49:20 -0400 Subject: [PATCH 4/9] Two new modules --- python/gtsam/examples/EKF_SLAM.ipynb | 404 +++++++------------------- python/gtsam/examples/gtsam_plotly.py | 290 ++++++++++++++++++ python/gtsam/examples/simulation.py | 137 +++++++++ 3 files changed, 527 insertions(+), 304 deletions(-) create mode 100644 python/gtsam/examples/gtsam_plotly.py create mode 100644 python/gtsam/examples/simulation.py diff --git a/python/gtsam/examples/EKF_SLAM.ipynb b/python/gtsam/examples/EKF_SLAM.ipynb index 9422204f1..84e69e1a1 100644 --- a/python/gtsam/examples/EKF_SLAM.ipynb +++ b/python/gtsam/examples/EKF_SLAM.ipynb @@ -62,12 +62,17 @@ " * Optimize the final Bayes Net to get the updated state estimate (`Values`).\n", "4. **Iteration:** Repeat prediction and update for each time step.\n", "\n", + "**Enhancements in this version:**\n", + "* Simulation code is moved to `simulation.py`.\n", + "* Plotting code is moved to `gtsam_plotly.py`.\n", + "* The evolving factor graph is displayed at each step using `graphviz`.\n", + "\n", "**Advantages:**\n", "* Leverages GTSAM's robust factor graph machinery.\n", "* Handles non-linearities through iterative linearization (like EKF, but potentially more robust within GTSAM's optimization context).\n", "* Avoids explicit management of large, dense covariance matrices.\n", "\n", - "**Output:** The process will be visualized using a Plotly animation showing the robot's estimated path, mapped landmarks, and uncertainty ellipses evolving over time." + "**Output:** The process will be visualized with the evolving factor graph, and finally using a Plotly animation showing the robot's estimated path, mapped landmarks, and uncertainty ellipses evolving over time." ] }, { @@ -101,13 +106,22 @@ "outputs": [], "source": [ "import numpy as np\n", - "import matplotlib.pyplot as plt # For initial plot if desired\n", + "import matplotlib.pyplot as plt # For initial plot if desired (optional)\n", "import plotly.graph_objects as go\n", "from tqdm.notebook import tqdm # Progress bar\n", "import math\n", + "import time # To slow down graphviz display if needed\n", "\n", "import gtsam\n", - "from gtsam.symbol_shorthand import X, L # Symbols for poses and landmarks" + "from gtsam.symbol_shorthand import X, L # Symbols for poses and landmarks\n", + "\n", + "# Imports for new modules\n", + "import simulation\n", + "import gtsam_plotly \n", + "\n", + "# Imports for graph visualization\n", + "import graphviz\n", + "from IPython.display import display, clear_output" ] }, { @@ -132,7 +146,7 @@ "# Robot parameters\n", "ROBOT_RADIUS = 3.0\n", "ROBOT_ANGULAR_VEL = np.deg2rad(20.0) # Radians per step\n", - "NUM_STEPS = 50 # Reduced steps for faster animation generation\n", + "NUM_STEPS = 20 # Reduced steps for faster animation generation\n", "DT = 1.0 # Time step duration (simplified)\n", "\n", "# Noise parameters (GTSAM Noise Models)\n", @@ -143,13 +157,12 @@ "# Measurement noise (bearing, range)\n", "MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([np.deg2rad(2.0), 0.2]))\n", "\n", - "# Create samplers for noise models\n", - "prior_noise_sampler = gtsam.Sampler(PRIOR_NOISE, 42)\n", - "odometry_noise_sampler = gtsam.Sampler(ODOMETRY_NOISE, 42)\n", - "measurement_noise_sampler = gtsam.Sampler(MEASUREMENT_NOISE, 42)\n", - "\n", "# Sensor parameters\n", - "MAX_SENSOR_RANGE = 5.0" + "MAX_SENSOR_RANGE = 5.0\n", + "\n", + "# Visualization parameters\n", + "SHOW_GRAPHVIZ_EACH_STEP = True # Set to False to only show final graph\n", + "GRAPHVIZ_PAUSE_SECONDS = 1 # Pause briefly to allow viewing the graph" ] }, { @@ -159,79 +172,30 @@ "source": [ "## 3. Generate Ground Truth Data\n", "\n", - "Create true landmark positions, robot path, and simulate noisy measurements." + "Use the `simulation` module to create true landmark positions, robot path, and simulate noisy measurements." ] }, { "cell_type": "code", "execution_count": null, - "id": "ground-truth-code-srif", + "id": "ground-truth-call-srif", "metadata": {}, "outputs": [], "source": [ - "# 3.1 Ground Truth Landmarks\n", - "landmarks_gt = (np.random.rand(2, NUM_LANDMARKS) - 0.5) * WORLD_SIZE\n", - "landmark_ids = list(range(NUM_LANDMARKS))\n", - "landmarks_gt_dict = {L(i): gtsam.Point2(landmarks_gt[:, i]) for i in range(NUM_LANDMARKS)}\n", - "\n", - "# 3.2 Ground Truth Robot Path\n", - "poses_gt = []\n", - "current_pose_gt = gtsam.Pose2(ROBOT_RADIUS, 0, np.pi / 2) # Start on circle edge\n", - "poses_gt.append(current_pose_gt)\n", - "\n", - "for k in range(NUM_STEPS):\n", - " delta_theta = ROBOT_ANGULAR_VEL * DT\n", - " arc_length = ROBOT_ANGULAR_VEL * ROBOT_RADIUS * DT\n", - " motion_command = gtsam.Pose2(arc_length, 0, delta_theta)\n", - " current_pose_gt = current_pose_gt.compose(motion_command)\n", - " poses_gt.append(current_pose_gt)\n", - "\n", - "# 3.3 Simulate Noisy Odometry Measurements (as Pose2 objects)\n", - "odometry_measurements = []\n", - "for k in range(NUM_STEPS):\n", - " pose_k = poses_gt[k]\n", - " pose_k1 = poses_gt[k+1]\n", - " true_odom = pose_k.between(pose_k1)\n", - " \n", - " # Sample noise directly for Pose2 composition (approximate)\n", - " # A more rigorous approach samples from the tangent space\n", - " odom_noise_vec = odometry_noise_sampler.sample()\n", - " noisy_odom = true_odom.compose(gtsam.Pose2(odom_noise_vec[0], odom_noise_vec[1], odom_noise_vec[2]))\n", - " odometry_measurements.append(noisy_odom)\n", - "\n", - "# 3.4 Simulate Noisy Bearing-Range Measurements\n", - "# measurements_sim[k] = list of (L(lm_id), range, bearing) for measurements *at* pose k\n", - "measurements_sim = [[] for _ in range(NUM_STEPS + 1)]\n", - "for k in range(NUM_STEPS + 1):\n", - " robot_pose = poses_gt[k]\n", - " for lm_id in range(NUM_LANDMARKS):\n", - " lm_gt_pt = landmarks_gt_dict[L(lm_id)]\n", - " try:\n", - " # Use BearingRangeFactor2D to simulate the measurement model\n", - " measurement_factor = gtsam.BearingRangeFactor2D(X(k), L(lm_id), \n", - " robot_pose.bearing(lm_gt_pt),\n", - " robot_pose.range(lm_gt_pt), \n", - " MEASUREMENT_NOISE)\n", - " true_range = measurement_factor.measured().range()\n", - " true_bearing = measurement_factor.measured().bearing()\n", - "\n", - " if true_range <= MAX_SENSOR_RANGE and abs(true_bearing.theta()) 0: # Ensure range is positive\n", - " measurements_sim[k].append((L(lm_id), noisy_bearing, noisy_range))\n", - " except Exception as e:\n", - " # Catch potential errors like point being too close to the pose\n", - " print(f\"Sim Warning at step {k}, landmark {lm_id}: {e}\")\n", - " pass\n", - "\n", - "print(f\"Generated {NUM_LANDMARKS} landmarks.\")\n", - "print(f\"Generated {NUM_STEPS} ground truth poses and odometry measurements.\")\n", - "num_meas_total = sum(len(m_list) for m_list in measurements_sim)\n", - "print(f\"Generated {num_meas_total} bearing-range measurements across all steps.\")" + "landmarks_gt_dict, poses_gt, odometry_measurements, measurements_sim, landmarks_gt_array = \\\n", + " simulation.generate_simulation_data(\n", + " num_landmarks=NUM_LANDMARKS,\n", + " world_size=WORLD_SIZE,\n", + " robot_radius=ROBOT_RADIUS,\n", + " robot_angular_vel=ROBOT_ANGULAR_VEL,\n", + " num_steps=NUM_STEPS,\n", + " dt=DT,\n", + " odometry_noise_model=ODOMETRY_NOISE,\n", + " measurement_noise_model=MEASUREMENT_NOISE,\n", + " max_sensor_range=MAX_SENSOR_RANGE,\n", + " X=X, # Pass symbol functions\n", + " L=L\n", + " )" ] }, { @@ -241,7 +205,25 @@ "source": [ "## 4. Iterative Factor Graph SLAM Implementation\n", "\n", - "Here we perform the step-by-step prediction and update using factor graphs." + "Here we perform the step-by-step prediction and update using factor graphs. The evolving graph will be displayed." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "90bf9320", + "metadata": {}, + "outputs": [], + "source": [ + "writer = gtsam.GraphvizFormatting()\n", + "writer.binaryEdges = True\n", + "\n", + "def maybe_show(graph, estimate, message):\n", + " if SHOW_GRAPHVIZ_EACH_STEP:\n", + " print(message)\n", + " graph_dot = graph.dot(estimate, writer=writer)\n", + " display(graphviz.Source(graph_dot, engine='neato'))\n", + " time.sleep(GRAPHVIZ_PAUSE_SECONDS)" ] }, { @@ -258,11 +240,15 @@ "\n", "# Add prior on the initial pose X(0)\n", "initial_pose = poses_gt[0] # Start at ground truth (or add noise)\n", - "current_estimate.insert(current_pose_key, initial_pose)\n", + "# Add a bit of noise to the initial guess if desired:\n", + "# initial_pose_noisy = initial_pose.compose(gtsam.Pose2(0.1, 0.1, 0.01))\n", + "# current_estimate.insert(current_pose_key, initial_pose_noisy)\n", + "current_estimate.insert(current_pose_key, initial_pose) \n", + "\n", "current_graph.add(gtsam.PriorFactorPose2(current_pose_key, initial_pose, PRIOR_NOISE))\n", "\n", "# Variables to store results for animation\n", - "results_history = [current_estimate] # Store Values object at each step\n", + "results_history = [gtsam.Values(current_estimate)] # Store Values object at each step\n", "marginals_history = [] # Store Marginals object at each step\n", "known_landmarks = set() # Set of landmark keys L(j) currently in the state\n", "\n", @@ -273,7 +259,9 @@ " marginals_history.append(initial_marginals)\n", "except Exception as e:\n", " print(f\"Error computing initial marginals: {e}\")\n", - " marginals_history.append(None) # Append placeholder if fails\n" + " marginals_history.append(None) # Append placeholder if fails\n", + "\n", + "maybe_show(current_graph, current_estimate, \"--- Step 0 --- Initial Graph with Prior ---\")" ] }, { @@ -300,18 +288,15 @@ " odom_k = odometry_measurements[k]\n", " \n", " # Predict next pose and add to Values (linearization point)\n", + " # Ensure previous pose exists before accessing\n", " predicted_pose = current_estimate.atPose2(prev_pose_key).compose(odom_k)\n", - " current_estimate.insert(current_pose_key, predicted_pose)\n", + " \n", + " # Insert or update the current pose prediction\n", + " current_estimate.insert_or_assign(current_pose_key, predicted_pose)\n", " \n", " # Add odometry factor to the graph\n", " current_graph.add(gtsam.BetweenFactorPose2(prev_pose_key, current_pose_key, odom_k, ODOMETRY_NOISE))\n", "\n", - " # --- At this point, the full graph contains the history --- \n", - " # --- To mimic SRIF/EKF, we *could* eliminate prev_pose_key, but --- \n", - " # --- it's simpler here to keep the full graph and optimize it --- \n", - " # --- then extract the current belief for the next step's factors --- \n", - " # --- Let's proceed with adding measurements first ----\n", - "\n", " # === Update Step ===\n", " measurements_k1 = measurements_sim[k + 1] # Measurements taken AT pose k+1\n", " landmarks_observed_this_step = set()\n", @@ -329,42 +314,29 @@ " lm_initial_guess = current_pose_val.transformFrom(gtsam.Point2(delta_x, delta_y))\n", " current_estimate.insert(lm_key, lm_initial_guess)\n", " known_landmarks.add(lm_key)\n", - " # print(f\"Step {k+1}: Initialized Landmark {lm_key.index()}\")\n", "\n", " # Add measurement factor\n", " current_graph.add(gtsam.BearingRangeFactor2D(current_pose_key, lm_key, \n", - " measured_bearing, measured_range,\n", - " MEASUREMENT_NOISE))\n", + " measured_bearing, measured_range,\n", + " MEASUREMENT_NOISE))\n", " \n", " # --- Optimization (Incremental Smoothing) ---\n", - " # In a true filter, we'd eliminate previous states.\n", - " # Here, we re-optimize the growing graph. For efficiency in large problems, \n", - " # iSAM2 would be used, but Levenberg-Marquardt on the growing graph \n", - " # demonstrates the concept for this smaller example.\n", - " \n", " optimizer = gtsam.LevenbergMarquardtOptimizer(current_graph, current_estimate)\n", - " try:\n", - " current_estimate = optimizer.optimize() # Update estimates by optimizing the whole graph so far\n", - " except Exception as e:\n", - " print(f\"Optimization failed at step {k+1}: {e}\")\n", - " # Potentially revert estimate update or handle error\n", - " # For simplicity, we continue with the potentially unoptimized estimate\n", - " pass \n", + " current_estimate = optimizer.optimize() # Update estimates by optimizing the whole graph so far\n", + " \n", + " # --- Display Factor Graph --- \n", + " maybe_show(current_graph, current_estimate, f\"--- Step {k+1} --- Factor Graph ---\")\n", "\n", " # Store results for animation\n", " results_history.append(gtsam.Values(current_estimate)) # Store a copy\n", " \n", " # Calculate marginals for visualization (can be slow for large graphs)\n", - " try:\n", - " current_gaussian_graph = current_graph.linearize(current_estimate)\n", - " current_marginals = gtsam.Marginals(current_gaussian_graph, current_estimate)\n", - " marginals_history.append(current_marginals)\n", - " except Exception as e:\n", - " print(f\"Marginals calculation failed at step {k+1}: {e}\")\n", - " marginals_history.append(None) # Append placeholder\n", + " current_gaussian_graph = current_graph.linearize(current_estimate)\n", + " current_marginals = gtsam.Marginals(current_gaussian_graph, current_estimate)\n", + " marginals_history.append(current_marginals)\n", "\n", - "print(\"Iterative Factor Graph SLAM finished.\")\n", - "print(f\"Final number of poses: {k+2}\")\n", + "print(\"\\nIterative Factor Graph SLAM finished.\")\n", + "print(f\"Final number of poses estimated: {len([key for key in current_estimate.keys() if gtsam.Symbol(key).chr() == ord('x')])}\")\n", "print(f\"Number of landmarks mapped: {len(known_landmarks)}\")" ] }, @@ -373,208 +345,30 @@ "id": "plotly-animation", "metadata": {}, "source": [ - "## 5. Create Plotly Animation" + "## 5. Create Plotly Animation\n", + "\n", + "Use the `gtsam_plotly` module to visualize the results." ] }, { "cell_type": "code", "execution_count": null, - "id": "plotly-animation-code", + "id": "plotly-animation-call-code", "metadata": { "scrolled": false, - "tags": [ - "remove-input" - ] + "tags": [] }, "outputs": [], "source": [ - "print(\"Generating Plotly animation...\")\n", - "\n", - "def ellipse_path(cx, cy, sizex, sizey, angle, N=60):\n", - " \"\"\"SVG path string for an ellipse centered at (cx, cy), rotated by `angle` in degrees.\"\"\"\n", - " angle_rad = np.radians(angle)\n", - " t = np.linspace(0, 2 * np.pi, N)\n", - " x = (sizex / 2) * np.cos(t)\n", - " y = (sizey / 2) * np.sin(t)\n", - "\n", - " x_rot = cx + x * np.cos(angle_rad) - y * np.sin(angle_rad)\n", - " y_rot = cy + x * np.sin(angle_rad) + y * np.cos(angle_rad)\n", - "\n", - " path = f\"M {x_rot[0]},{y_rot[0]} \" + \" \".join(\n", - " f\"L{x_},{y_}\" for x_, y_ in zip(x_rot[1:], y_rot[1:])\n", - " ) + \" Z\"\n", - " return path\n", - "\n", - "# Helper to convert GTSAM covariance to Plotly ellipse parameters\n", - "def gtsam_cov_to_plotly_ellipse(cov_matrix, scale=2.0):\n", - " \"\"\"Calculates ellipse parameters (angle, width, height) from a 2x2 covariance matrix.\"\"\"\n", - " # Ensure positive definite - add small epsilon if needed\n", - " cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 \n", - " try:\n", - " eigvals, eigvecs = np.linalg.eigh(cov)\n", - " # Ensure eigenvalues are positive for sqrt\n", - " eigvals = np.maximum(eigvals, 1e-9)\n", - " except np.linalg.LinAlgError:\n", - " print(\"Warning: Covariance matrix SVD failed, using default ellipse.\")\n", - " return 0, 0.1*scale, 0.1*scale # Default small ellipse\n", - " \n", - " # Width/Height are 2*scale*sqrt(eigenvalue)\n", - " width = 2 * scale * np.sqrt(eigvals[1])\n", - " height = 2 * scale * np.sqrt(eigvals[0])\n", - " \n", - " # Angle of the major axis (corresponding to largest eigenvalue)\n", - " angle_rad = np.arctan2(eigvecs[1, 1], eigvecs[0, 1])\n", - " angle_deg = np.degrees(angle_rad)\n", - " \n", - " return angle_deg, width, height\n", - "\n", - "# --- Create Plotly Figure --- \n", - "fig = go.Figure()\n", - "\n", - "# Add Ground Truth Landmarks (static)\n", - "fig.add_trace(go.Scatter(\n", - " x=landmarks_gt[0, :], \n", - " y=landmarks_gt[1, :], \n", - " mode='markers', \n", - " marker=dict(color='black', size=8, symbol='star'),\n", - " name='Landmarks GT'\n", - "))\n", - "\n", - "# Add Ground Truth Path (static)\n", - "gt_path_x = [p.x() for p in poses_gt]\n", - "gt_path_y = [p.y() for p in poses_gt]\n", - "fig.add_trace(go.Scatter(\n", - " x=gt_path_x,\n", - " y=gt_path_y,\n", - " mode='lines',\n", - " line=dict(color='gray', width=1, dash='dash'),\n", - " name='Path GT'\n", - "))\n", - "\n", - "# --- Animation Frames --- \n", - "frames = []\n", - "steps = list(range(NUM_STEPS + 1))\n", - "\n", - "for k in tqdm(steps):\n", - " frame_data = []\n", - " step_results = results_history[k]\n", - " step_marginals = marginals_history[k]\n", - " \n", - " # Estimated Path up to step k\n", - " est_path_x = [results_history[i].atPose2(X(i)).x() for i in range(k + 1)]\n", - " est_path_y = [results_history[i].atPose2(X(i)).y() for i in range(k + 1)]\n", - " frame_data.append(go.Scatter(\n", - " x=est_path_x,\n", - " y=est_path_y,\n", - " mode='lines+markers',\n", - " line=dict(color='red', width=2),\n", - " marker=dict(size=4, color='red'),\n", - " name='Path Est'\n", - " ))\n", - " \n", - " # Estimated Landmarks known at step k\n", - " est_landmarks_x = []\n", - " est_landmarks_y = []\n", - " landmark_keys_in_frame = []\n", - " for lm_key in step_results.keys():\n", - " if gtsam.symbolChr(lm_key)==108: # Check if it's a landmark key\n", - " lm_pose = step_results.atPoint2(lm_key)\n", - " est_landmarks_x.append(lm_pose[0])\n", - " est_landmarks_y.append(lm_pose[1])\n", - " landmark_keys_in_frame.append(lm_key)\n", - " \n", - " if est_landmarks_x:\n", - " frame_data.append(go.Scatter(\n", - " x=est_landmarks_x, \n", - " y=est_landmarks_y, \n", - " mode='markers',\n", - " marker=dict(color='blue', size=6, symbol='x'),\n", - " name='Landmarks Est'\n", - " ))\n", - "\n", - " # Current Pose Covariance Ellipse\n", - " shapes = [] # List to hold ellipse shapes for this frame\n", - " if step_marginals is not None:\n", - " try:\n", - " current_pose_key = X(k)\n", - " pose_cov = step_marginals.marginalCovariance(current_pose_key)\n", - " pose_mean = step_results.atPose2(current_pose_key).translation()\n", - " angle, width, height = gtsam_cov_to_plotly_ellipse(pose_cov)\n", - " cx, cy = pose_mean[0], pose_mean[1]\n", - " shapes.append(dict(\n", - " type=\"path\",\n", - " path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle, N=60),\n", - " xref=\"x\", yref=\"y\",\n", - " fillcolor=\"rgba(255,0,255,0.2)\",\n", - " line_color=\"rgba(255,0,255,0.5)\",\n", - " name=f'Pose {k} Cov'\n", - " ))\n", - " except Exception as e:\n", - " print(f\"Warning: Failed getting pose cov ellipse at step {k}: {e}\")\n", - "\n", - " # Landmark Covariance Ellipses\n", - " for lm_key in landmark_keys_in_frame:\n", - " index = gtsam.symbolIndex(lm_key)\n", - " try:\n", - " lm_cov = step_marginals.marginalCovariance(lm_key)\n", - " lm_mean = step_results.atPoint2(lm_key)\n", - " angle, width, height = gtsam_cov_to_plotly_ellipse(lm_cov)\n", - " cx, cy = lm_mean[0], lm_mean[1]\n", - " shapes.append(dict(\n", - " type=\"path\",\n", - " path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle, N=60),\n", - " xref=\"x\", yref=\"y\",\n", - " fillcolor=\"rgba(0,0,255,0.1)\",\n", - " line_color=\"rgba(0,0,255,0.3)\",\n", - " name=f'LM {index} Cov'\n", - " ))\n", - " except Exception as e:\n", - " print(f\"Warning: Failed getting landmark {index} cov ellipse at step {k}: {e}\")\n", - "\n", - " frames.append(go.Frame(data=frame_data, name=str(k), layout=go.Layout(shapes=shapes)))\n", - "\n", - "# --- Set Initial State and Layout --- \n", - "fig.update(frames=frames)\n", - "\n", - "# Set initial data to the first frame's data\n", - "if frames:\n", - " fig.add_traces(frames[0].data)\n", - " initial_shapes = frames[0].layout.shapes if frames[0].layout else []\n", - "else:\n", - " initial_shapes = []\n", - "\n", - "# Define slider\n", - "sliders = [dict(\n", - " active=0,\n", - " currentvalue={\"prefix\": \"Step: \"},\n", - " pad={\"t\": 50},\n", - " steps=[dict(label=str(k), \n", - " method='animate', \n", - " args=[[str(k)], \n", - " dict(mode='immediate', \n", - " frame=dict(duration=100, redraw=True), \n", - " transition=dict(duration=0))])\n", - " for k in steps]\n", - ")]\n", - "\n", - "# Update layout\n", - "fig.update_layout(\n", - " title='Iterative Factor Graph SLAM Animation',\n", - " xaxis=dict(range=[-WORLD_SIZE/2 - 2, WORLD_SIZE/2 + 2], constrain='domain'),\n", - " yaxis=dict(range=[-WORLD_SIZE/2 - 2, WORLD_SIZE/2 + 2], scaleanchor='x', scaleratio=1),\n", - " width=800, height=800,\n", - " hovermode='closest',\n", - " updatemenus=[dict(type='buttons', \n", - " showactive=False,\n", - " buttons=[dict(label='Play',\n", - " method='animate', \n", - " args=[None, \n", - " dict(mode='immediate', \n", - " frame=dict(duration=100, redraw=True), \n", - " transition=dict(duration=0), \n", - " fromcurrent=True)])])],\n", - " sliders=sliders,\n", - " shapes=initial_shapes # Set initial shapes\n", + "fig = gtsam_plotly.create_slam_animation(\n", + " results_history=results_history,\n", + " marginals_history=marginals_history,\n", + " landmarks_gt_array=landmarks_gt_array,\n", + " poses_gt=poses_gt,\n", + " num_steps=NUM_STEPS,\n", + " world_size=WORLD_SIZE,\n", + " X=X, # Pass symbol functions\n", + " L=L\n", ")\n", "\n", "print(\"Displaying animation...\")\n", @@ -589,6 +383,8 @@ "## 6. Discussion\n", "\n", "* **Approach:** This notebook implemented SLAM iteratively using GTSAM factor graphs. At each step, new factors (odometry, measurements) were added, and the graph was re-optimized using Levenberg-Marquardt. This is more akin to **incremental smoothing** than a classic filter (which would explicitly marginalize past states).\n", + "* **Modularity:** Simulation and Plotly visualization code have been moved into separate `simulation.py` and `gtsam_plotly.py` files for better organization.\n", + "* **Graph Visualization:** The `graphviz` library was used to render the factor graph at each step (or only at the end, depending on `SHOW_GRAPHVIZ_EACH_STEP`). This helps visualize how the graph structure grows and connects poses and landmarks.\n", "* **Efficiency:** Optimizing the entire graph at every step becomes computationally expensive for long trajectories. For real-time performance or large problems, **iSAM2 (Incremental Smoothing and Mapping)** is the preferred GTSAM algorithm. iSAM2 efficiently updates the solution by only re-linearizing and re-solving parts of the graph affected by new measurements.\n", "* **Accuracy vs. EKF:** This factor graph approach generally handles non-linearities better than a standard EKF because it re-linearizes during optimization. It avoids some of the consistency pitfalls associated with the EKF's single linearization point per step.\n", "* **Visualization:** The Plotly animation shows the evolution of the robot's path estimate, the map of landmarks, and their associated uncertainties (covariance ellipses). You can observe how measurements help refine the estimates and reduce uncertainty, especially when loops are closed (implicitly here through repeated observations of landmarks)." @@ -597,7 +393,7 @@ ], "metadata": { "kernelspec": { - "display_name": "Python 3 (ipykernel)", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -611,7 +407,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.10.12" + "version": "3.12.6" } }, "nbformat": 4, diff --git a/python/gtsam/examples/gtsam_plotly.py b/python/gtsam/examples/gtsam_plotly.py new file mode 100644 index 000000000..398156263 --- /dev/null +++ b/python/gtsam/examples/gtsam_plotly.py @@ -0,0 +1,290 @@ +# gtsam_plotly.py +import numpy as np +import plotly.graph_objects as go +from tqdm.notebook import tqdm # Progress bar + +import gtsam + + +def ellipse_path(cx, cy, sizex, sizey, angle, N=60): + """SVG path string for an ellipse centered at (cx, cy), rotated by `angle` in degrees.""" + angle_rad = np.radians(angle) + t = np.linspace(0, 2 * np.pi, N) + x = (sizex / 2) * np.cos(t) + y = (sizey / 2) * np.sin(t) + + x_rot = cx + x * np.cos(angle_rad) - y * np.sin(angle_rad) + y_rot = cy + x * np.sin(angle_rad) + y * np.cos(angle_rad) + + path = ( + f"M {x_rot[0]},{y_rot[0]} " + + " ".join(f"L{x_},{y_}" for x_, y_ in zip(x_rot[1:], y_rot[1:])) + + " Z" + ) + return path + + +# Helper to convert GTSAM covariance to Plotly ellipse parameters +def gtsam_cov_to_plotly_ellipse(cov_matrix, scale=2.0): + """Calculates ellipse parameters (angle, width, height) from a 2x2 covariance matrix.""" + # Ensure positive definite - add small epsilon if needed + cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 + try: + eigvals, eigvecs = np.linalg.eigh(cov) + # Ensure eigenvalues are positive for sqrt + eigvals = np.maximum(eigvals, 1e-9) + except np.linalg.LinAlgError: + # print("Warning: Covariance matrix SVD failed, using default ellipse.") # Can be verbose + return 0, 0.1 * scale, 0.1 * scale # Default small ellipse + + # Width/Height are 2*scale*sqrt(eigenvalue) + width = 2 * scale * np.sqrt(eigvals[1]) + height = 2 * scale * np.sqrt(eigvals[0]) + + # Angle of the major axis (corresponding to largest eigenvalue) + angle_rad = np.arctan2(eigvecs[1, 1], eigvecs[0, 1]) + angle_deg = np.degrees(angle_rad) + + return angle_deg, width, height + + +def create_slam_animation( + results_history, # List of gtsam.Values + marginals_history, # List of gtsam.Marginals or None + landmarks_gt_array, # Nx2 numpy array + poses_gt, # List of gtsam.Pose2 + num_steps, + world_size, + X, # Symbol func + L, # Symbol func +): + """Creates a Plotly animation of the SLAM results.""" + print("Generating Plotly animation...") + + fig = go.Figure() + + # Add Ground Truth Landmarks (static) + fig.add_trace( + go.Scatter( + x=landmarks_gt_array[0, :], + y=landmarks_gt_array[1, :], + mode="markers", + marker=dict(color="black", size=8, symbol="star"), + name="Landmarks GT", + ) + ) + + # Add Ground Truth Path (static) + gt_path_x = [p.x() for p in poses_gt] + gt_path_y = [p.y() for p in poses_gt] + fig.add_trace( + go.Scatter( + x=gt_path_x, + y=gt_path_y, + mode="lines", + line=dict(color="gray", width=1, dash="dash"), + name="Path GT", + ) + ) + + # --- Animation Frames --- + frames = [] + steps = list(range(num_steps + 1)) + + for k in tqdm(steps): + frame_data = [] + step_results = results_history[k] + step_marginals = marginals_history[k] + + # Estimated Path up to step k + est_path_x = [] + est_path_y = [] + for i in range(k + 1): + pose_key = X(i) + if step_results.exists( + pose_key + ): # Check if pose exists in results for this step + pose = step_results.atPose2(pose_key) + est_path_x.append(pose.x()) + est_path_y.append(pose.y()) + + frame_data.append( + go.Scatter( + x=est_path_x, + y=est_path_y, + mode="lines+markers", + line=dict(color="red", width=2), + marker=dict(size=4, color="red"), + name="Path Est", + ) + ) + + # Estimated Landmarks known at step k + est_landmarks_x = [] + est_landmarks_y = [] + landmark_keys_in_frame = [] + # Iterate through all possible landmark keys seen so far, check if present in current step's results + all_keys = step_results.keys() + for lm_key_val in all_keys: # Use keys() for integer keys + if gtsam.Symbol(lm_key_val).chr() == ord("l"): # Check if symbol chr is 'l' + lm_key = lm_key_val # Use the integer key directly + if step_results.exists(lm_key): + lm_pose = step_results.atPoint2(lm_key) + est_landmarks_x.append(lm_pose[0]) + est_landmarks_y.append(lm_pose[1]) + landmark_keys_in_frame.append(lm_key) + + if est_landmarks_x: + frame_data.append( + go.Scatter( + x=est_landmarks_x, + y=est_landmarks_y, + mode="markers", + marker=dict(color="blue", size=6, symbol="x"), + name="Landmarks Est", + ) + ) + + # Covariance Ellipses + shapes = [] # List to hold ellipse shapes for this frame + if step_marginals is not None: + # Current Pose Covariance Ellipse + try: + current_pose_key = X(k) + if step_results.exists(current_pose_key): + pose_cov = step_marginals.marginalCovariance(current_pose_key) + pose_mean = step_results.atPose2(current_pose_key).translation() + angle, width, height = gtsam_cov_to_plotly_ellipse(pose_cov) + cx, cy = pose_mean[0], pose_mean[1] + shapes.append( + dict( + type="path", + path=ellipse_path( + cx=cx, + cy=cy, + sizex=width, + sizey=height, + angle=angle, + N=60, + ), + xref="x", + yref="y", + fillcolor="rgba(255,0,255,0.2)", + line_color="rgba(255,0,255,0.5)", + name=f"Pose {k} Cov", + ) + ) + except Exception as e: + print( + f"Warning: Failed getting pose {k} cov ellipse at step {k}: {e}" + ) # Can be verbose + pass + + # Landmark Covariance Ellipses + for lm_key in landmark_keys_in_frame: + try: + lm_cov = step_marginals.marginalCovariance(lm_key) + lm_mean = step_results.atPoint2(lm_key) + angle, width, height = gtsam_cov_to_plotly_ellipse(lm_cov) + cx, cy = lm_mean[0], lm_mean[1] + index = gtsam.Symbol(lm_key).index() + shapes.append( + dict( + type="path", + path=ellipse_path( + cx=cx, + cy=cy, + sizex=width, + sizey=height, + angle=angle, + N=60, + ), + xref="x", + yref="y", + fillcolor="rgba(0,0,255,0.1)", + line_color="rgba(0,0,255,0.3)", + name=f"LM {index} Cov", + ) + ) + except Exception as e: + index = gtsam.Symbol(lm_key).index() + print( + f"Warning: Failed getting landmark {index} cov ellipse at step {k}: {e}" + ) # Can be verbose + + frames.append( + go.Frame(data=frame_data, name=str(k), layout=go.Layout(shapes=shapes)) + ) + + # --- Set Initial State and Layout --- + fig.update(frames=frames) + + # Set initial data to the first frame's data + if frames: + fig.add_traces(frames[0].data) + initial_shapes = frames[0].layout.shapes if frames[0].layout else [] + else: + initial_shapes = [] + + # Define slider + sliders = [ + dict( + active=0, + currentvalue={"prefix": "Step: "}, + pad={"t": 50}, + steps=[ + dict( + label=str(k), + method="animate", + args=[ + [str(k)], + dict( + mode="immediate", + frame=dict(duration=100, redraw=True), + transition=dict(duration=0), + ), + ], + ) + for k in steps + ], + ) + ] + + # Update layout + fig.update_layout( + title="Iterative Factor Graph SLAM Animation", + xaxis=dict(range=[-world_size / 2 - 2, world_size / 2 + 2], constrain="domain"), + yaxis=dict( + range=[-world_size / 2 - 2, world_size / 2 + 2], + scaleanchor="x", + scaleratio=1, + ), + width=800, + height=800, + hovermode="closest", + updatemenus=[ + dict( + type="buttons", + showactive=False, + buttons=[ + dict( + label="Play", + method="animate", + args=[ + None, + dict( + mode="immediate", + frame=dict(duration=100, redraw=True), + transition=dict(duration=0), + fromcurrent=True, + ), + ], + ) + ], + ) + ], + sliders=sliders, + shapes=initial_shapes, # Set initial shapes + ) + print("Plotly animation generated.") + return fig diff --git a/python/gtsam/examples/simulation.py b/python/gtsam/examples/simulation.py new file mode 100644 index 000000000..a36433878 --- /dev/null +++ b/python/gtsam/examples/simulation.py @@ -0,0 +1,137 @@ +# simulation.py +import numpy as np + +import gtsam + + +def generate_simulation_data( + num_landmarks, + world_size, + robot_radius, + robot_angular_vel, + num_steps, + dt, + odometry_noise_model, + measurement_noise_model, + max_sensor_range, + X, # Symbol generator function + L, # Symbol generator function + odom_seed=42, + meas_seed=42, + landmark_seed=42, +): + """Generates ground truth and simulated measurements for SLAM. + + Args: + num_landmarks: Number of landmarks to generate. + world_size: Size of the square world environment. + robot_radius: Radius of the robot's circular path. + robot_angular_vel: Angular velocity of the robot (rad/step). + num_steps: Number of simulation steps. + dt: Time step duration. + odometry_noise_model: GTSAM noise model for odometry. + measurement_noise_model: GTSAM noise model for bearing-range. + max_sensor_range: Maximum range of the bearing-range sensor. + X: GTSAM symbol shorthand function for poses. + L: GTSAM symbol shorthand function for landmarks. + odom_seed: Random seed for odometry noise. + meas_seed: Random seed for measurement noise. + landmark_seed: Random seed for landmark placement. + + Returns: + tuple: Contains: + - landmarks_gt_dict (dict): L(i) -> gtsam.Point2 ground truth. + - poses_gt (list): List of gtsam.Pose2 ground truth poses. + - odometry_measurements (list): List of noisy gtsam.Pose2 odometry. + - measurements_sim (list): List of lists, measurements_sim[k] contains + tuples (L(lm_id), bearing, range) for step k. + - landmarks_gt_array (np.array): 2xN numpy array of landmark positions. + """ + np.random.seed(landmark_seed) + odometry_noise_sampler = gtsam.Sampler(odometry_noise_model, odom_seed) + measurement_noise_sampler = gtsam.Sampler(measurement_noise_model, meas_seed) + + # 1. Ground Truth Landmarks + landmarks_gt_array = (np.random.rand(2, num_landmarks) - 0.5) * world_size + landmarks_gt_dict = { + L(i): gtsam.Point2(landmarks_gt_array[:, i]) for i in range(num_landmarks) + } + + # 2. Ground Truth Robot Path + poses_gt = [] + current_pose_gt = gtsam.Pose2(robot_radius, 0, np.pi / 2) # Start on circle edge + poses_gt.append(current_pose_gt) + + for _ in range(num_steps): + delta_theta = robot_angular_vel * dt + arc_length = robot_angular_vel * robot_radius * dt + motion_command = gtsam.Pose2(arc_length, 0, delta_theta) + current_pose_gt = current_pose_gt.compose(motion_command) + poses_gt.append(current_pose_gt) + + # 3. Simulate Noisy Odometry Measurements + odometry_measurements = [] + for k in range(num_steps): + pose_k = poses_gt[k] + pose_k1 = poses_gt[k + 1] + true_odom = pose_k.between(pose_k1) + + # Sample noise directly for Pose2 composition (approximate) + odom_noise_vec = odometry_noise_sampler.sample() + noisy_odom = true_odom.compose( + gtsam.Pose2(odom_noise_vec[0], odom_noise_vec[1], odom_noise_vec[2]) + ) + odometry_measurements.append(noisy_odom) + + # 4. Simulate Noisy Bearing-Range Measurements + measurements_sim = [[] for _ in range(num_steps + 1)] + for k in range(num_steps + 1): + robot_pose = poses_gt[k] + for lm_id in range(num_landmarks): + lm_gt_pt = landmarks_gt_dict[L(lm_id)] + try: + measurement_factor = gtsam.BearingRangeFactor2D( + X(k), + L(lm_id), + robot_pose.bearing(lm_gt_pt), + robot_pose.range(lm_gt_pt), + measurement_noise_model, + ) + true_range = measurement_factor.measured().range() + true_bearing = measurement_factor.measured().bearing() + + # Check sensor limits (range and Field of View - e.g. +/- 45 degrees) + if ( + true_range <= max_sensor_range + and abs(true_bearing.theta()) < np.pi / 4 + ): + # Sample noise + noise_vec = measurement_noise_sampler.sample() + noisy_bearing = true_bearing.retract( + np.array([noise_vec[0]]) + ) # Retract on SO(2) + noisy_range = true_range + noise_vec[1] + + if noisy_range > 0: # Ensure range is positive + measurements_sim[k].append( + (L(lm_id), noisy_bearing, noisy_range) + ) + except Exception as e: + # Catch potential errors like point being too close to the pose + # print(f"Sim Warning at step {k}, landmark {lm_id}: {e}") # Can be verbose + pass + + print(f"Simulation Generated: {num_landmarks} landmarks.") + print( + f"Simulation Generated: {num_steps + 1} ground truth poses and {num_steps} odometry measurements." + ) + num_meas_total = sum(len(m_list) for m_list in measurements_sim) + print(f"Simulation Generated: {num_meas_total} bearing-range measurements.") + + return ( + landmarks_gt_dict, + poses_gt, + odometry_measurements, + measurements_sim, + landmarks_gt_array, + ) From 8148b78af621e2cda699cb2d8e26c27031993b33 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Apr 2025 15:51:56 -0400 Subject: [PATCH 5/9] More modular --- python/gtsam/examples/gtsam_plotly.py | 593 +++++++++++++++++--------- 1 file changed, 385 insertions(+), 208 deletions(-) diff --git a/python/gtsam/examples/gtsam_plotly.py b/python/gtsam/examples/gtsam_plotly.py index 398156263..5cf4f4462 100644 --- a/python/gtsam/examples/gtsam_plotly.py +++ b/python/gtsam/examples/gtsam_plotly.py @@ -1,13 +1,31 @@ -# gtsam_plotly.py +# gtsam_plotly_modular.py import numpy as np import plotly.graph_objects as go from tqdm.notebook import tqdm # Progress bar +from typing import List, Optional, Tuple, Dict, Any import gtsam +# --- Ellipse Calculation Helpers (Mostly unchanged) --- -def ellipse_path(cx, cy, sizex, sizey, angle, N=60): - """SVG path string for an ellipse centered at (cx, cy), rotated by `angle` in degrees.""" + +def ellipse_path( + cx: float, cy: float, sizex: float, sizey: float, angle: float, N: int = 60 +) -> str: + """ + Generates an SVG path string for an ellipse. + + Args: + cx: Center x-coordinate. + cy: Center y-coordinate. + sizex: Full width of the ellipse along its major axis. + sizey: Full height of the ellipse along its minor axis. + angle: Rotation angle in degrees. + N: Number of points to approximate the ellipse. + + Returns: + SVG path string. + """ angle_rad = np.radians(angle) t = np.linspace(0, 2 * np.pi, N) x = (sizex / 2) * np.cos(t) @@ -24,9 +42,19 @@ def ellipse_path(cx, cy, sizex, sizey, angle, N=60): return path -# Helper to convert GTSAM covariance to Plotly ellipse parameters -def gtsam_cov_to_plotly_ellipse(cov_matrix, scale=2.0): - """Calculates ellipse parameters (angle, width, height) from a 2x2 covariance matrix.""" +def gtsam_cov_to_plotly_ellipse( + cov_matrix: np.ndarray, scale: float = 2.0 +) -> Tuple[float, float, float]: + """ + Calculates ellipse parameters (angle, width, height) from a 2x2 covariance matrix. + + Args: + cov_matrix: The 2x2 covariance matrix (or larger, only top-left 2x2 used). + scale: Scaling factor for the ellipse size (e.g., 2.0 for 2-sigma). + + Returns: + Tuple containing (angle_degrees, width, height). + """ # Ensure positive definite - add small epsilon if needed cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 try: @@ -34,199 +62,225 @@ def gtsam_cov_to_plotly_ellipse(cov_matrix, scale=2.0): # Ensure eigenvalues are positive for sqrt eigvals = np.maximum(eigvals, 1e-9) except np.linalg.LinAlgError: - # print("Warning: Covariance matrix SVD failed, using default ellipse.") # Can be verbose + # print("Warning: Covariance matrix SVD failed, using default ellipse.") # Optional warning return 0, 0.1 * scale, 0.1 * scale # Default small ellipse - # Width/Height are 2*scale*sqrt(eigenvalue) - width = 2 * scale * np.sqrt(eigvals[1]) - height = 2 * scale * np.sqrt(eigvals[0]) + # Width/Height are 2*scale*sqrt(eigenvalue) (using full width/height) + width = ( + 2 * scale * np.sqrt(eigvals[1]) + ) # Major axis corresponds to largest eigenvalue + height = ( + 2 * scale * np.sqrt(eigvals[0]) + ) # Minor axis corresponds to smallest eigenvalue - # Angle of the major axis (corresponding to largest eigenvalue) + # Angle of the major axis (eigenvector corresponding to largest eigenvalue eigvals[1]) angle_rad = np.arctan2(eigvecs[1, 1], eigvecs[0, 1]) angle_deg = np.degrees(angle_rad) return angle_deg, width, height -def create_slam_animation( - results_history, # List of gtsam.Values - marginals_history, # List of gtsam.Marginals or None - landmarks_gt_array, # Nx2 numpy array - poses_gt, # List of gtsam.Pose2 - num_steps, - world_size, - X, # Symbol func - L, # Symbol func -): - """Creates a Plotly animation of the SLAM results.""" - print("Generating Plotly animation...") +# --- Plotting Element Creation Helpers --- - fig = go.Figure() - # Add Ground Truth Landmarks (static) - fig.add_trace( +def _add_ground_truth_traces( + fig: go.Figure, landmarks_gt_array: np.ndarray, poses_gt: List[gtsam.Pose2] +) -> None: + """Adds static ground truth landmark and path traces to the figure.""" + # Ground Truth Landmarks + if landmarks_gt_array is not None and landmarks_gt_array.size > 0: + fig.add_trace( + go.Scatter( + x=landmarks_gt_array[0, :], + y=landmarks_gt_array[1, :], + mode="markers", + marker=dict(color="black", size=8, symbol="star"), + name="Landmarks GT", + ) + ) + + # Ground Truth Path + if poses_gt: + gt_path_x = [p.x() for p in poses_gt] + gt_path_y = [p.y() for p in poses_gt] + fig.add_trace( + go.Scatter( + x=gt_path_x, + y=gt_path_y, + mode="lines", + line=dict(color="gray", width=1, dash="dash"), + name="Path GT", + ) + ) + + +def _create_ellipse_shape_dict( + cx: float, + cy: float, + angle: float, + width: float, + height: float, + fillcolor: str, + line_color: str, + name: str, +) -> Dict[str, Any]: + """Creates the dictionary required for a Plotly ellipse shape.""" + return dict( + type="path", + path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle, N=60), + xref="x", + yref="y", + fillcolor=fillcolor, + line_color=line_color, + name=name, # Note: name isn't directly displayed for shapes, but good for metadata + ) + + +def _create_single_frame_data( + k: int, + step_results: gtsam.Values, + step_marginals: Optional[gtsam.Marginals], + X: callable, + L: callable, + ellipse_scale: float = 2.0, + verbose: bool = False, +) -> Tuple[List[go.Scatter], List[Dict[str, Any]]]: + """ + Creates the traces and shapes for a single animation frame. + + Args: + k: The current step index. + step_results: gtsam.Values for this step. + step_marginals: gtsam.Marginals for this step (or None). + X: Symbol function for poses. + L: Symbol function for landmarks. + ellipse_scale: Scaling factor for covariance ellipses. + verbose: If True, print warnings for covariance errors. + + Returns: + A tuple containing (list_of_traces, list_of_shapes). + """ + traces = [] + shapes = [] + + # 1. Estimated Path up to step k + est_path_x = [] + est_path_y = [] + for i in range(k + 1): + pose_key = X(i) + if step_results.exists(pose_key): + pose = step_results.atPose2(pose_key) + est_path_x.append(pose.x()) + est_path_y.append(pose.y()) + + traces.append( go.Scatter( - x=landmarks_gt_array[0, :], - y=landmarks_gt_array[1, :], - mode="markers", - marker=dict(color="black", size=8, symbol="star"), - name="Landmarks GT", + x=est_path_x, + y=est_path_y, + mode="lines+markers", + line=dict(color="red", width=2), + marker=dict(size=4, color="red"), + name="Path Est", # Legend entry for the whole path ) ) - # Add Ground Truth Path (static) - gt_path_x = [p.x() for p in poses_gt] - gt_path_y = [p.y() for p in poses_gt] - fig.add_trace( - go.Scatter( - x=gt_path_x, - y=gt_path_y, - mode="lines", - line=dict(color="gray", width=1, dash="dash"), - name="Path GT", - ) - ) + # 2. Estimated Landmarks known at step k + est_landmarks_x = [] + est_landmarks_y = [] + landmark_keys_in_frame = [] + all_keys = step_results.keys() + for key_val in all_keys: + symbol = gtsam.Symbol(key_val) + if symbol.chr() == ord("l"): # Check if it's a landmark symbol + # Check existence again (though keys() implies existence) + if step_results.exists(key_val): + lm_point = step_results.atPoint2(key_val) + est_landmarks_x.append(lm_point[0]) + est_landmarks_y.append(lm_point[1]) + landmark_keys_in_frame.append(key_val) + + if est_landmarks_x: + traces.append( + go.Scatter( + x=est_landmarks_x, + y=est_landmarks_y, + mode="markers", + marker=dict(color="blue", size=6, symbol="x"), + name="Landmarks Est", # Legend entry for all estimated landmarks + ) + ) + + # 3. Covariance Ellipses (if marginals available) + if step_marginals is not None: + # Current Pose Covariance Ellipse + current_pose_key = X(k) + if step_results.exists(current_pose_key): + try: + pose_cov = step_marginals.marginalCovariance(current_pose_key) + pose_mean = step_results.atPose2(current_pose_key).translation() + angle, width, height = gtsam_cov_to_plotly_ellipse( + pose_cov, scale=ellipse_scale + ) + shapes.append( + _create_ellipse_shape_dict( + cx=pose_mean[0], + cy=pose_mean[1], + angle=angle, + width=width, + height=height, + fillcolor="rgba(255,0,255,0.2)", + line_color="rgba(255,0,255,0.5)", + name=f"Pose {k} Cov", + ) + ) + except Exception as e: + if verbose: + print( + f"Warning: Failed getting pose {k} cov ellipse at step {k}: {e}" + ) + + # Landmark Covariance Ellipses + for lm_key in landmark_keys_in_frame: + try: + lm_cov = step_marginals.marginalCovariance(lm_key) + lm_mean = step_results.atPoint2(lm_key) + angle, width, height = gtsam_cov_to_plotly_ellipse( + lm_cov, scale=ellipse_scale + ) + symbol = gtsam.Symbol(lm_key) + shapes.append( + _create_ellipse_shape_dict( + cx=lm_mean[0], + cy=lm_mean[1], + angle=angle, + width=width, + height=height, + fillcolor="rgba(0,0,255,0.1)", + line_color="rgba(0,0,255,0.3)", + name=f"LM {symbol.index()} Cov", + ) + ) + except Exception as e: + symbol = gtsam.Symbol(lm_key) + if verbose: + print( + f"Warning: Failed getting landmark {symbol.index()} cov ellipse at step {k}: {e}" + ) + + return traces, shapes + + +def _configure_figure_layout( + fig: go.Figure, + num_steps: int, + world_size: float, + initial_shapes: List[Dict[str, Any]], +) -> None: + """Configures the Plotly figure's layout, axes, slider, and buttons.""" - # --- Animation Frames --- - frames = [] steps = list(range(num_steps + 1)) - for k in tqdm(steps): - frame_data = [] - step_results = results_history[k] - step_marginals = marginals_history[k] - - # Estimated Path up to step k - est_path_x = [] - est_path_y = [] - for i in range(k + 1): - pose_key = X(i) - if step_results.exists( - pose_key - ): # Check if pose exists in results for this step - pose = step_results.atPose2(pose_key) - est_path_x.append(pose.x()) - est_path_y.append(pose.y()) - - frame_data.append( - go.Scatter( - x=est_path_x, - y=est_path_y, - mode="lines+markers", - line=dict(color="red", width=2), - marker=dict(size=4, color="red"), - name="Path Est", - ) - ) - - # Estimated Landmarks known at step k - est_landmarks_x = [] - est_landmarks_y = [] - landmark_keys_in_frame = [] - # Iterate through all possible landmark keys seen so far, check if present in current step's results - all_keys = step_results.keys() - for lm_key_val in all_keys: # Use keys() for integer keys - if gtsam.Symbol(lm_key_val).chr() == ord("l"): # Check if symbol chr is 'l' - lm_key = lm_key_val # Use the integer key directly - if step_results.exists(lm_key): - lm_pose = step_results.atPoint2(lm_key) - est_landmarks_x.append(lm_pose[0]) - est_landmarks_y.append(lm_pose[1]) - landmark_keys_in_frame.append(lm_key) - - if est_landmarks_x: - frame_data.append( - go.Scatter( - x=est_landmarks_x, - y=est_landmarks_y, - mode="markers", - marker=dict(color="blue", size=6, symbol="x"), - name="Landmarks Est", - ) - ) - - # Covariance Ellipses - shapes = [] # List to hold ellipse shapes for this frame - if step_marginals is not None: - # Current Pose Covariance Ellipse - try: - current_pose_key = X(k) - if step_results.exists(current_pose_key): - pose_cov = step_marginals.marginalCovariance(current_pose_key) - pose_mean = step_results.atPose2(current_pose_key).translation() - angle, width, height = gtsam_cov_to_plotly_ellipse(pose_cov) - cx, cy = pose_mean[0], pose_mean[1] - shapes.append( - dict( - type="path", - path=ellipse_path( - cx=cx, - cy=cy, - sizex=width, - sizey=height, - angle=angle, - N=60, - ), - xref="x", - yref="y", - fillcolor="rgba(255,0,255,0.2)", - line_color="rgba(255,0,255,0.5)", - name=f"Pose {k} Cov", - ) - ) - except Exception as e: - print( - f"Warning: Failed getting pose {k} cov ellipse at step {k}: {e}" - ) # Can be verbose - pass - - # Landmark Covariance Ellipses - for lm_key in landmark_keys_in_frame: - try: - lm_cov = step_marginals.marginalCovariance(lm_key) - lm_mean = step_results.atPoint2(lm_key) - angle, width, height = gtsam_cov_to_plotly_ellipse(lm_cov) - cx, cy = lm_mean[0], lm_mean[1] - index = gtsam.Symbol(lm_key).index() - shapes.append( - dict( - type="path", - path=ellipse_path( - cx=cx, - cy=cy, - sizex=width, - sizey=height, - angle=angle, - N=60, - ), - xref="x", - yref="y", - fillcolor="rgba(0,0,255,0.1)", - line_color="rgba(0,0,255,0.3)", - name=f"LM {index} Cov", - ) - ) - except Exception as e: - index = gtsam.Symbol(lm_key).index() - print( - f"Warning: Failed getting landmark {index} cov ellipse at step {k}: {e}" - ) # Can be verbose - - frames.append( - go.Frame(data=frame_data, name=str(k), layout=go.Layout(shapes=shapes)) - ) - - # --- Set Initial State and Layout --- - fig.update(frames=frames) - - # Set initial data to the first frame's data - if frames: - fig.add_traces(frames[0].data) - initial_shapes = frames[0].layout.shapes if frames[0].layout else [] - else: - initial_shapes = [] - - # Define slider + # Slider sliders = [ dict( active=0, @@ -237,10 +291,12 @@ def create_slam_animation( label=str(k), method="animate", args=[ - [str(k)], + [str(k)], # Frame name dict( mode="immediate", - frame=dict(duration=100, redraw=True), + frame=dict( + duration=100, redraw=True + ), # Redraw needed for shapes transition=dict(duration=0), ), ], @@ -250,41 +306,162 @@ def create_slam_animation( ) ] - # Update layout + # Buttons + updatemenus = [ + dict( + type="buttons", + showactive=False, + buttons=[ + dict( + label="Play", + method="animate", + args=[ + None, # Animate all frames + dict( + mode="immediate", + frame=dict(duration=100, redraw=True), + transition=dict(duration=0), + fromcurrent=True, + ), + ], + ), + dict( + label="Pause", + method="animate", + args=[ + [None], # Stop animation + dict( + mode="immediate", + frame=dict(duration=0, redraw=False), + transition=dict(duration=0), + ), + ], + ), + ], + direction="left", + pad={"r": 10, "t": 87}, + x=0.1, + xanchor="right", + y=0, + yanchor="top", + ) + ] + + # Layout settings fig.update_layout( title="Iterative Factor Graph SLAM Animation", - xaxis=dict(range=[-world_size / 2 - 2, world_size / 2 + 2], constrain="domain"), + xaxis=dict( + range=[-world_size / 2 - 2, world_size / 2 + 2], + constrain="domain", # Keep aspect ratio when zooming + ), yaxis=dict( range=[-world_size / 2 - 2, world_size / 2 + 2], - scaleanchor="x", + scaleanchor="x", # Ensure square aspect ratio scaleratio=1, ), width=800, height=800, hovermode="closest", - updatemenus=[ - dict( - type="buttons", - showactive=False, - buttons=[ - dict( - label="Play", - method="animate", - args=[ - None, - dict( - mode="immediate", - frame=dict(duration=100, redraw=True), - transition=dict(duration=0), - fromcurrent=True, - ), - ], - ) - ], - ) - ], + updatemenus=updatemenus, sliders=sliders, - shapes=initial_shapes, # Set initial shapes + shapes=initial_shapes, # Set initial shapes from frame 0 + # Add legend if desired + legend=dict( + traceorder="reversed", + title_text="Legend", + orientation="h", + yanchor="bottom", + y=1.02, + xanchor="right", + x=1, + ), ) + + +# --- Main Animation Function (Orchestrator) --- + + +def create_slam_animation( + results_history: List[gtsam.Values], + marginals_history: List[Optional[gtsam.Marginals]], + num_steps: int, + X: callable, + L: callable, + landmarks_gt_array: Optional[np.ndarray] = None, + poses_gt: Optional[List[gtsam.Pose2]] = None, + world_size: float = 20.0, + ellipse_scale: float = 2.0, + verbose_cov_errors: bool = False, +) -> go.Figure: + """ + Creates a Plotly animation of the SLAM results in a modular way. + + Args: + results_history: List of gtsam.Values, one per step. + marginals_history: List of gtsam.Marginals or None, one per step. + num_steps: The total number of steps (results_history should have length num_steps + 1). + X: Symbol function for poses (e.g., lambda i: gtsam.symbol('x', i)). + L: Symbol function for landmarks (e.g., lambda j: gtsam.symbol('l', j)). + landmarks_gt_array: Optional Nx2 numpy array of ground truth landmark positions. + poses_gt: Optional list of gtsam.Pose2 ground truth poses. + world_size: Approximate size of the world for axis scaling. + ellipse_scale: Scaling factor for covariance ellipses (e.g., 2.0 for 2-sigma). + verbose_cov_errors: If True, print warnings for covariance calculation errors. + + Returns: + A plotly.graph_objects.Figure containing the animation. + """ + print("Generating Plotly animation...") + + fig = go.Figure() + + # 1. Add static ground truth elements + _add_ground_truth_traces(fig, landmarks_gt_array, poses_gt) + + # 2. Create frames for animation + frames = [] + steps_iterable = range(num_steps + 1) + + # Use tqdm for progress bar if available + try: + steps_iterable = tqdm(steps_iterable, desc="Creating Frames") + except NameError: + pass # tqdm not installed or not in notebook env + + for k in steps_iterable: + step_results = results_history[k] + step_marginals = marginals_history[k] if marginals_history else None + + # Create traces and shapes for this specific frame + frame_traces, frame_shapes = _create_single_frame_data( + k, step_results, step_marginals, X, L, ellipse_scale, verbose_cov_errors + ) + + # Create the Plotly frame object + frames.append( + go.Frame( + data=frame_traces, + name=str(k), # Name used by slider/buttons + layout=go.Layout( + shapes=frame_shapes + ), # Shapes are part of layout per frame + ) + ) + + # 3. Set initial figure state (using data from frame 0) + if frames: + # Add traces from the first frame as the initial state + for trace in frames[0].data: + fig.add_trace(trace) + initial_shapes = frames[0].layout.shapes if frames[0].layout else [] + else: + initial_shapes = [] + + # 4. Assign frames to the figure + fig.update(frames=frames) + + # 5. Configure overall layout, slider, buttons + _configure_figure_layout(fig, num_steps, world_size, initial_shapes) + print("Plotly animation generated.") return fig From 1bf76be62b5480ad901fa7022937635e06470244 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Apr 2025 15:58:33 -0400 Subject: [PATCH 6/9] Even more modular --- python/gtsam/examples/gtsam_plotly.py | 458 ++++++++++++-------------- 1 file changed, 202 insertions(+), 256 deletions(-) diff --git a/python/gtsam/examples/gtsam_plotly.py b/python/gtsam/examples/gtsam_plotly.py index 5cf4f4462..d37e32dc9 100644 --- a/python/gtsam/examples/gtsam_plotly.py +++ b/python/gtsam/examples/gtsam_plotly.py @@ -1,39 +1,25 @@ -# gtsam_plotly_modular.py +# gtsam_plotly_modular_v2.py +from typing import Any, Callable, Dict, List, Optional, Tuple + import numpy as np import plotly.graph_objects as go -from tqdm.notebook import tqdm # Progress bar -from typing import List, Optional, Tuple, Dict, Any +from tqdm.notebook import tqdm import gtsam -# --- Ellipse Calculation Helpers (Mostly unchanged) --- +# --- Core Ellipse Calculations --- def ellipse_path( cx: float, cy: float, sizex: float, sizey: float, angle: float, N: int = 60 ) -> str: - """ - Generates an SVG path string for an ellipse. - - Args: - cx: Center x-coordinate. - cy: Center y-coordinate. - sizex: Full width of the ellipse along its major axis. - sizey: Full height of the ellipse along its minor axis. - angle: Rotation angle in degrees. - N: Number of points to approximate the ellipse. - - Returns: - SVG path string. - """ + """Generates SVG path string for a rotated ellipse.""" angle_rad = np.radians(angle) t = np.linspace(0, 2 * np.pi, N) - x = (sizex / 2) * np.cos(t) - y = (sizey / 2) * np.sin(t) - - x_rot = cx + x * np.cos(angle_rad) - y * np.sin(angle_rad) - y_rot = cy + x * np.sin(angle_rad) + y * np.cos(angle_rad) - + x_unit = (sizex / 2) * np.cos(t) + y_unit = (sizey / 2) * np.sin(t) + x_rot = cx + x_unit * np.cos(angle_rad) - y_unit * np.sin(angle_rad) + y_rot = cy + x_unit * np.sin(angle_rad) + y_unit * np.cos(angle_rad) path = ( f"M {x_rot[0]},{y_rot[0]} " + " ".join(f"L{x_},{y_}" for x_, y_ in zip(x_rot[1:], y_rot[1:])) @@ -45,125 +31,150 @@ def ellipse_path( def gtsam_cov_to_plotly_ellipse( cov_matrix: np.ndarray, scale: float = 2.0 ) -> Tuple[float, float, float]: - """ - Calculates ellipse parameters (angle, width, height) from a 2x2 covariance matrix. - - Args: - cov_matrix: The 2x2 covariance matrix (or larger, only top-left 2x2 used). - scale: Scaling factor for the ellipse size (e.g., 2.0 for 2-sigma). - - Returns: - Tuple containing (angle_degrees, width, height). - """ - # Ensure positive definite - add small epsilon if needed - cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 + """Calculates ellipse angle (deg), width, height from 2x2 covariance.""" + cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 # Ensure positive definite try: eigvals, eigvecs = np.linalg.eigh(cov) - # Ensure eigenvalues are positive for sqrt - eigvals = np.maximum(eigvals, 1e-9) + eigvals = np.maximum(eigvals, 1e-9) # Ensure positive eigenvalues except np.linalg.LinAlgError: - # print("Warning: Covariance matrix SVD failed, using default ellipse.") # Optional warning - return 0, 0.1 * scale, 0.1 * scale # Default small ellipse + return 0, 0.1 * scale, 0.1 * scale # Default on failure - # Width/Height are 2*scale*sqrt(eigenvalue) (using full width/height) - width = ( - 2 * scale * np.sqrt(eigvals[1]) - ) # Major axis corresponds to largest eigenvalue - height = ( - 2 * scale * np.sqrt(eigvals[0]) - ) # Minor axis corresponds to smallest eigenvalue - - # Angle of the major axis (eigenvector corresponding to largest eigenvalue eigvals[1]) - angle_rad = np.arctan2(eigvecs[1, 1], eigvecs[0, 1]) + width = 2 * scale * np.sqrt(eigvals[1]) # Major axis (largest eigenvalue) + height = 2 * scale * np.sqrt(eigvals[0]) # Minor axis (smallest eigenvalue) + angle_rad = np.arctan2( + eigvecs[1, 1], eigvecs[0, 1] + ) # Angle of major axis eigenvector angle_deg = np.degrees(angle_rad) - return angle_deg, width, height -# --- Plotting Element Creation Helpers --- +# --- Plotly Element Generators --- -def _add_ground_truth_traces( - fig: go.Figure, landmarks_gt_array: np.ndarray, poses_gt: List[gtsam.Pose2] -) -> None: - """Adds static ground truth landmark and path traces to the figure.""" - # Ground Truth Landmarks - if landmarks_gt_array is not None and landmarks_gt_array.size > 0: - fig.add_trace( - go.Scatter( - x=landmarks_gt_array[0, :], - y=landmarks_gt_array[1, :], - mode="markers", - marker=dict(color="black", size=8, symbol="star"), - name="Landmarks GT", - ) - ) +def create_gt_landmarks_trace(landmarks_gt_array: np.ndarray) -> Optional[go.Scatter]: + """Creates scatter trace for ground truth landmarks.""" + if landmarks_gt_array is None or landmarks_gt_array.size == 0: + return None + return go.Scatter( + x=landmarks_gt_array[0, :], + y=landmarks_gt_array[1, :], + mode="markers", + marker=dict(color="black", size=8, symbol="star"), + name="Landmarks GT", + ) - # Ground Truth Path - if poses_gt: - gt_path_x = [p.x() for p in poses_gt] - gt_path_y = [p.y() for p in poses_gt] - fig.add_trace( - go.Scatter( - x=gt_path_x, - y=gt_path_y, - mode="lines", - line=dict(color="gray", width=1, dash="dash"), - name="Path GT", - ) - ) + +def create_gt_path_trace(poses_gt: List[gtsam.Pose2]) -> Optional[go.Scatter]: + """Creates line trace for ground truth path.""" + if not poses_gt: + return None + gt_path_x = [p.x() for p in poses_gt] + gt_path_y = [p.y() for p in poses_gt] + return go.Scatter( + x=gt_path_x, + y=gt_path_y, + mode="lines", + line=dict(color="gray", width=1, dash="dash"), + name="Path GT", + ) + + +def create_est_path_trace( + est_path_x: List[float], est_path_y: List[float] +) -> go.Scatter: + """Creates scatter/line trace for the estimated path up to current step.""" + return go.Scatter( + x=est_path_x, + y=est_path_y, + mode="lines+markers", + line=dict(color="red", width=2), + marker=dict(size=4, color="red"), + name="Path Est", # This name applies to the trace in the specific frame + ) + + +def create_est_landmarks_trace( + est_landmarks_x: List[float], est_landmarks_y: List[float] +) -> Optional[go.Scatter]: + """Creates scatter trace for currently estimated landmarks.""" + if not est_landmarks_x: + return None + return go.Scatter( + x=est_landmarks_x, + y=est_landmarks_y, + mode="markers", + marker=dict(color="blue", size=6, symbol="x"), + name="Landmarks Est", # Applies to landmarks in the specific frame + ) def _create_ellipse_shape_dict( - cx: float, - cy: float, - angle: float, - width: float, - height: float, - fillcolor: str, - line_color: str, - name: str, + cx, cy, angle, width, height, fillcolor, line_color, name_suffix ) -> Dict[str, Any]: - """Creates the dictionary required for a Plotly ellipse shape.""" + """Helper to create the dictionary for a Plotly ellipse shape.""" return dict( type="path", - path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle, N=60), + path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle), xref="x", yref="y", fillcolor=fillcolor, line_color=line_color, - name=name, # Note: name isn't directly displayed for shapes, but good for metadata + # name=f"{name_suffix} Cov", # Name isn't really used by Plotly for shapes ) -def _create_single_frame_data( +def create_pose_ellipse_shape( + pose_mean_xy: np.ndarray, pose_cov: np.ndarray, k: int, scale: float +) -> Dict[str, Any]: + """Creates shape dictionary for a pose covariance ellipse.""" + angle, width, height = gtsam_cov_to_plotly_ellipse(pose_cov, scale) + return _create_ellipse_shape_dict( + cx=pose_mean_xy[0], + cy=pose_mean_xy[1], + angle=angle, + width=width, + height=height, + fillcolor="rgba(255,0,255,0.2)", + line_color="rgba(255,0,255,0.5)", + name_suffix=f"Pose {k}", + ) + + +def create_landmark_ellipse_shape( + lm_mean_xy: np.ndarray, lm_cov: np.ndarray, lm_index: int, scale: float +) -> Dict[str, Any]: + """Creates shape dictionary for a landmark covariance ellipse.""" + angle, width, height = gtsam_cov_to_plotly_ellipse(lm_cov, scale) + return _create_ellipse_shape_dict( + cx=lm_mean_xy[0], + cy=lm_mean_xy[1], + angle=angle, + width=width, + height=height, + fillcolor="rgba(0,0,255,0.1)", + line_color="rgba(0,0,255,0.3)", + name_suffix=f"LM {lm_index}", + ) + + +# --- Frame Content Generation --- + + +def generate_frame_content( k: int, step_results: gtsam.Values, step_marginals: Optional[gtsam.Marginals], - X: callable, - L: callable, + X: Callable[[int], int], + L: Callable[[int], int], + max_landmark_index: int, # Need to know the potential range of landmarks ellipse_scale: float = 2.0, verbose: bool = False, ) -> Tuple[List[go.Scatter], List[Dict[str, Any]]]: - """ - Creates the traces and shapes for a single animation frame. + """Generates all dynamic traces and shapes for a single animation frame `k`.""" + frame_traces: List[go.Scatter] = [] + frame_shapes: List[Dict[str, Any]] = [] - Args: - k: The current step index. - step_results: gtsam.Values for this step. - step_marginals: gtsam.Marginals for this step (or None). - X: Symbol function for poses. - L: Symbol function for landmarks. - ellipse_scale: Scaling factor for covariance ellipses. - verbose: If True, print warnings for covariance errors. - - Returns: - A tuple containing (list_of_traces, list_of_shapes). - """ - traces = [] - shapes = [] - - # 1. Estimated Path up to step k + # 1. Gather Estimated Path Data est_path_x = [] est_path_y = [] for i in range(k + 1): @@ -172,115 +183,70 @@ def _create_single_frame_data( pose = step_results.atPose2(pose_key) est_path_x.append(pose.x()) est_path_y.append(pose.y()) + frame_traces.append(create_est_path_trace(est_path_x, est_path_y)) - traces.append( - go.Scatter( - x=est_path_x, - y=est_path_y, - mode="lines+markers", - line=dict(color="red", width=2), - marker=dict(size=4, color="red"), - name="Path Est", # Legend entry for the whole path - ) - ) - - # 2. Estimated Landmarks known at step k + # 2. Gather Estimated Landmark Data est_landmarks_x = [] est_landmarks_y = [] landmark_keys_in_frame = [] - all_keys = step_results.keys() - for key_val in all_keys: - symbol = gtsam.Symbol(key_val) - if symbol.chr() == ord("l"): # Check if it's a landmark symbol - # Check existence again (though keys() implies existence) - if step_results.exists(key_val): - lm_point = step_results.atPoint2(key_val) - est_landmarks_x.append(lm_point[0]) - est_landmarks_y.append(lm_point[1]) - landmark_keys_in_frame.append(key_val) + # Check all potential landmark keys up to max_landmark_index + for j in range(max_landmark_index + 1): + lm_key = L(j) + if step_results.exists(lm_key): + lm_point = step_results.atPoint2(lm_key) + est_landmarks_x.append(lm_point[0]) + est_landmarks_y.append(lm_point[1]) + landmark_keys_in_frame.append(lm_key) - if est_landmarks_x: - traces.append( - go.Scatter( - x=est_landmarks_x, - y=est_landmarks_y, - mode="markers", - marker=dict(color="blue", size=6, symbol="x"), - name="Landmarks Est", # Legend entry for all estimated landmarks - ) - ) + lm_trace = create_est_landmarks_trace(est_landmarks_x, est_landmarks_y) + if lm_trace: + frame_traces.append(lm_trace) - # 3. Covariance Ellipses (if marginals available) + # 3. Generate Covariance Ellipses (if marginals available) if step_marginals is not None: - # Current Pose Covariance Ellipse + # Pose ellipse current_pose_key = X(k) if step_results.exists(current_pose_key): try: pose_cov = step_marginals.marginalCovariance(current_pose_key) pose_mean = step_results.atPose2(current_pose_key).translation() - angle, width, height = gtsam_cov_to_plotly_ellipse( - pose_cov, scale=ellipse_scale - ) - shapes.append( - _create_ellipse_shape_dict( - cx=pose_mean[0], - cy=pose_mean[1], - angle=angle, - width=width, - height=height, - fillcolor="rgba(255,0,255,0.2)", - line_color="rgba(255,0,255,0.5)", - name=f"Pose {k} Cov", - ) + frame_shapes.append( + create_pose_ellipse_shape(pose_mean, pose_cov, k, ellipse_scale) ) except Exception as e: if verbose: - print( - f"Warning: Failed getting pose {k} cov ellipse at step {k}: {e}" - ) + print(f"Warn: Pose {k} cov err @ step {k}: {e}") - # Landmark Covariance Ellipses + # Landmark ellipses for lm_key in landmark_keys_in_frame: try: lm_cov = step_marginals.marginalCovariance(lm_key) lm_mean = step_results.atPoint2(lm_key) - angle, width, height = gtsam_cov_to_plotly_ellipse( - lm_cov, scale=ellipse_scale - ) - symbol = gtsam.Symbol(lm_key) - shapes.append( - _create_ellipse_shape_dict( - cx=lm_mean[0], - cy=lm_mean[1], - angle=angle, - width=width, - height=height, - fillcolor="rgba(0,0,255,0.1)", - line_color="rgba(0,0,255,0.3)", - name=f"LM {symbol.index()} Cov", + lm_index = gtsam.Symbol(lm_key).index() + frame_shapes.append( + create_landmark_ellipse_shape( + lm_mean, lm_cov, lm_index, ellipse_scale ) ) except Exception as e: - symbol = gtsam.Symbol(lm_key) + lm_index = gtsam.Symbol(lm_key).index() if verbose: - print( - f"Warning: Failed getting landmark {symbol.index()} cov ellipse at step {k}: {e}" - ) + print(f"Warn: LM {lm_index} cov err @ step {k}: {e}") - return traces, shapes + return frame_traces, frame_shapes -def _configure_figure_layout( +# --- Figure Configuration --- + + +def configure_figure_layout( fig: go.Figure, num_steps: int, world_size: float, initial_shapes: List[Dict[str, Any]], ) -> None: - """Configures the Plotly figure's layout, axes, slider, and buttons.""" - + """Configures Plotly figure layout, axes, slider, buttons.""" steps = list(range(num_steps + 1)) - - # Slider sliders = [ dict( active=0, @@ -291,12 +257,10 @@ def _configure_figure_layout( label=str(k), method="animate", args=[ - [str(k)], # Frame name + [str(k)], dict( mode="immediate", - frame=dict( - duration=100, redraw=True - ), # Redraw needed for shapes + frame=dict(duration=100, redraw=True), transition=dict(duration=0), ), ], @@ -305,18 +269,22 @@ def _configure_figure_layout( ], ) ] - - # Buttons updatemenus = [ dict( type="buttons", showactive=False, + direction="left", + pad={"r": 10, "t": 87}, + x=0.1, + xanchor="right", + y=0, + yanchor="top", buttons=[ dict( label="Play", method="animate", args=[ - None, # Animate all frames + None, dict( mode="immediate", frame=dict(duration=100, redraw=True), @@ -329,7 +297,7 @@ def _configure_figure_layout( label="Pause", method="animate", args=[ - [None], # Stop animation + [None], dict( mode="immediate", frame=dict(duration=0, redraw=False), @@ -338,25 +306,15 @@ def _configure_figure_layout( ], ), ], - direction="left", - pad={"r": 10, "t": 87}, - x=0.1, - xanchor="right", - y=0, - yanchor="top", ) ] - # Layout settings fig.update_layout( title="Iterative Factor Graph SLAM Animation", - xaxis=dict( - range=[-world_size / 2 - 2, world_size / 2 + 2], - constrain="domain", # Keep aspect ratio when zooming - ), + xaxis=dict(range=[-world_size / 2 - 2, world_size / 2 + 2], constrain="domain"), yaxis=dict( range=[-world_size / 2 - 2, world_size / 2 + 2], - scaleanchor="x", # Ensure square aspect ratio + scaleanchor="x", scaleratio=1, ), width=800, @@ -364,8 +322,7 @@ def _configure_figure_layout( hovermode="closest", updatemenus=updatemenus, sliders=sliders, - shapes=initial_shapes, # Set initial shapes from frame 0 - # Add legend if desired + shapes=initial_shapes, legend=dict( traceorder="reversed", title_text="Legend", @@ -378,90 +335,79 @@ def _configure_figure_layout( ) -# --- Main Animation Function (Orchestrator) --- +# --- Main Animation Orchestrator --- def create_slam_animation( results_history: List[gtsam.Values], marginals_history: List[Optional[gtsam.Marginals]], num_steps: int, - X: callable, - L: callable, + X: Callable[[int], int], + L: Callable[[int], int], + max_landmark_index: int, # Required to iterate potential landmarks landmarks_gt_array: Optional[np.ndarray] = None, poses_gt: Optional[List[gtsam.Pose2]] = None, world_size: float = 20.0, ellipse_scale: float = 2.0, verbose_cov_errors: bool = False, ) -> go.Figure: - """ - Creates a Plotly animation of the SLAM results in a modular way. - - Args: - results_history: List of gtsam.Values, one per step. - marginals_history: List of gtsam.Marginals or None, one per step. - num_steps: The total number of steps (results_history should have length num_steps + 1). - X: Symbol function for poses (e.g., lambda i: gtsam.symbol('x', i)). - L: Symbol function for landmarks (e.g., lambda j: gtsam.symbol('l', j)). - landmarks_gt_array: Optional Nx2 numpy array of ground truth landmark positions. - poses_gt: Optional list of gtsam.Pose2 ground truth poses. - world_size: Approximate size of the world for axis scaling. - ellipse_scale: Scaling factor for covariance ellipses (e.g., 2.0 for 2-sigma). - verbose_cov_errors: If True, print warnings for covariance calculation errors. - - Returns: - A plotly.graph_objects.Figure containing the animation. - """ + """Creates a modular Plotly SLAM animation.""" print("Generating Plotly animation...") - fig = go.Figure() - # 1. Add static ground truth elements - _add_ground_truth_traces(fig, landmarks_gt_array, poses_gt) + # 1. Add static ground truth traces to the base figure (visible always) + gt_lm_trace = create_gt_landmarks_trace(landmarks_gt_array) + if gt_lm_trace: + fig.add_trace(gt_lm_trace) + gt_path_trace = create_gt_path_trace(poses_gt) + if gt_path_trace: + fig.add_trace(gt_path_trace) - # 2. Create frames for animation + # 2. Generate frames with dynamic content frames = [] steps_iterable = range(num_steps + 1) - - # Use tqdm for progress bar if available try: steps_iterable = tqdm(steps_iterable, desc="Creating Frames") except NameError: - pass # tqdm not installed or not in notebook env + pass # tqdm optional for k in steps_iterable: step_results = results_history[k] step_marginals = marginals_history[k] if marginals_history else None - # Create traces and shapes for this specific frame - frame_traces, frame_shapes = _create_single_frame_data( - k, step_results, step_marginals, X, L, ellipse_scale, verbose_cov_errors + frame_traces, frame_shapes = generate_frame_content( + k, + step_results, + step_marginals, + X, + L, + max_landmark_index, + ellipse_scale, + verbose_cov_errors, ) - - # Create the Plotly frame object frames.append( go.Frame( - data=frame_traces, - name=str(k), # Name used by slider/buttons - layout=go.Layout( - shapes=frame_shapes - ), # Shapes are part of layout per frame + data=frame_traces, name=str(k), layout=go.Layout(shapes=frame_shapes) ) ) - # 3. Set initial figure state (using data from frame 0) + # 3. Set initial dynamic data (from frame 0) onto the base figure + initial_dynamic_traces = [] + initial_shapes = [] if frames: - # Add traces from the first frame as the initial state - for trace in frames[0].data: - fig.add_trace(trace) + # Important: Add *copies* or ensure traces are regenerated if needed, + # though Plotly usually handles this ok with frame data. + initial_dynamic_traces = frames[0].data initial_shapes = frames[0].layout.shapes if frames[0].layout else [] - else: - initial_shapes = [] + for trace in initial_dynamic_traces: + fig.add_trace(trace) # Add Est Path[0], Est Landmarks[0] traces # 4. Assign frames to the figure fig.update(frames=frames) - # 5. Configure overall layout, slider, buttons - _configure_figure_layout(fig, num_steps, world_size, initial_shapes) + # 5. Configure layout, axes, controls + # Pass initial_shapes for the layout's starting state + configure_figure_layout(fig, num_steps, world_size, initial_shapes) print("Plotly animation generated.") return fig From e4278687b412c60e78bfc50c0b2c396b38553920 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Thu, 24 Apr 2025 16:42:23 -0400 Subject: [PATCH 7/9] Plot side by side --- python/gtsam/examples/EKF_SLAM.ipynb | 63 ++--- python/gtsam/examples/gtsam_plotly.py | 357 +++++++++++++++----------- 2 files changed, 221 insertions(+), 199 deletions(-) diff --git a/python/gtsam/examples/EKF_SLAM.ipynb b/python/gtsam/examples/EKF_SLAM.ipynb index 84e69e1a1..265005710 100644 --- a/python/gtsam/examples/EKF_SLAM.ipynb +++ b/python/gtsam/examples/EKF_SLAM.ipynb @@ -106,8 +106,6 @@ "outputs": [], "source": [ "import numpy as np\n", - "import matplotlib.pyplot as plt # For initial plot if desired (optional)\n", - "import plotly.graph_objects as go\n", "from tqdm.notebook import tqdm # Progress bar\n", "import math\n", "import time # To slow down graphviz display if needed\n", @@ -117,11 +115,10 @@ "\n", "# Imports for new modules\n", "import simulation\n", - "import gtsam_plotly \n", + "from gtsam_plotly import SlamFrameData, create_slam_animation\n", "\n", "# Imports for graph visualization\n", - "import graphviz\n", - "from IPython.display import display, clear_output" + "import graphviz" ] }, { @@ -158,11 +155,7 @@ "MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([np.deg2rad(2.0), 0.2]))\n", "\n", "# Sensor parameters\n", - "MAX_SENSOR_RANGE = 5.0\n", - "\n", - "# Visualization parameters\n", - "SHOW_GRAPHVIZ_EACH_STEP = True # Set to False to only show final graph\n", - "GRAPHVIZ_PAUSE_SECONDS = 1 # Pause briefly to allow viewing the graph" + "MAX_SENSOR_RANGE = 5.0" ] }, { @@ -215,15 +208,11 @@ "metadata": {}, "outputs": [], "source": [ - "writer = gtsam.GraphvizFormatting()\n", - "writer.binaryEdges = True\n", + "WRITER = gtsam.GraphvizFormatting()\n", + "WRITER.binaryEdges = True\n", "\n", - "def maybe_show(graph, estimate, message):\n", - " if SHOW_GRAPHVIZ_EACH_STEP:\n", - " print(message)\n", - " graph_dot = graph.dot(estimate, writer=writer)\n", - " display(graphviz.Source(graph_dot, engine='neato'))\n", - " time.sleep(GRAPHVIZ_PAUSE_SECONDS)" + "def mage_dot(graph, estimate):\n", + " return graph.dot(estimate, writer=WRITER)\n" ] }, { @@ -248,20 +237,13 @@ "current_graph.add(gtsam.PriorFactorPose2(current_pose_key, initial_pose, PRIOR_NOISE))\n", "\n", "# Variables to store results for animation\n", - "results_history = [gtsam.Values(current_estimate)] # Store Values object at each step\n", - "marginals_history = [] # Store Marginals object at each step\n", + "history = [] # Store SLAMFrameData objects for each step\n", "known_landmarks = set() # Set of landmark keys L(j) currently in the state\n", "\n", "# Initial marginals (only for X(0))\n", - "try:\n", - " initial_gaussian_graph = current_graph.linearize(current_estimate)\n", - " initial_marginals = gtsam.Marginals(initial_gaussian_graph, current_estimate)\n", - " marginals_history.append(initial_marginals)\n", - "except Exception as e:\n", - " print(f\"Error computing initial marginals: {e}\")\n", - " marginals_history.append(None) # Append placeholder if fails\n", - "\n", - "maybe_show(current_graph, current_estimate, \"--- Step 0 --- Initial Graph with Prior ---\")" + "initial_gaussian_graph = current_graph.linearize(current_estimate)\n", + "initial_marginals = gtsam.Marginals(initial_gaussian_graph, current_estimate)\n", + "history.append(SlamFrameData(0, current_estimate, initial_marginals, mage_dot(current_graph, current_estimate)))" ] }, { @@ -324,16 +306,12 @@ " optimizer = gtsam.LevenbergMarquardtOptimizer(current_graph, current_estimate)\n", " current_estimate = optimizer.optimize() # Update estimates by optimizing the whole graph so far\n", " \n", - " # --- Display Factor Graph --- \n", - " maybe_show(current_graph, current_estimate, f\"--- Step {k+1} --- Factor Graph ---\")\n", - "\n", - " # Store results for animation\n", - " results_history.append(gtsam.Values(current_estimate)) # Store a copy\n", - " \n", " # Calculate marginals for visualization (can be slow for large graphs)\n", " current_gaussian_graph = current_graph.linearize(current_estimate)\n", " current_marginals = gtsam.Marginals(current_gaussian_graph, current_estimate)\n", - " marginals_history.append(current_marginals)\n", + "\n", + " # Store the current state for visualization\n", + " history.append(SlamFrameData(k+1, current_estimate, current_marginals, mage_dot(current_graph, current_estimate)))\n", "\n", "print(\"\\nIterative Factor Graph SLAM finished.\")\n", "print(f\"Final number of poses estimated: {len([key for key in current_estimate.keys() if gtsam.Symbol(key).chr() == ord('x')])}\")\n", @@ -360,15 +338,14 @@ }, "outputs": [], "source": [ - "fig = gtsam_plotly.create_slam_animation(\n", - " results_history=results_history,\n", - " marginals_history=marginals_history,\n", + "fig = create_slam_animation(\n", + " history,\n", " landmarks_gt_array=landmarks_gt_array,\n", " poses_gt=poses_gt,\n", - " num_steps=NUM_STEPS,\n", " world_size=WORLD_SIZE,\n", - " X=X, # Pass symbol functions\n", - " L=L\n", + " X=X, # Pass symbol functions\n", + " L=L,\n", + " max_landmark_index=NUM_LANDMARKS,\n", ")\n", "\n", "print(\"Displaying animation...\")\n", @@ -384,7 +361,7 @@ "\n", "* **Approach:** This notebook implemented SLAM iteratively using GTSAM factor graphs. At each step, new factors (odometry, measurements) were added, and the graph was re-optimized using Levenberg-Marquardt. This is more akin to **incremental smoothing** than a classic filter (which would explicitly marginalize past states).\n", "* **Modularity:** Simulation and Plotly visualization code have been moved into separate `simulation.py` and `gtsam_plotly.py` files for better organization.\n", - "* **Graph Visualization:** The `graphviz` library was used to render the factor graph at each step (or only at the end, depending on `SHOW_GRAPHVIZ_EACH_STEP`). This helps visualize how the graph structure grows and connects poses and landmarks.\n", + "* **Graph Visualization:** The `graphviz` library was used to render the factor graph at each step. This helps visualize how the graph structure grows and connects poses and landmarks.\n", "* **Efficiency:** Optimizing the entire graph at every step becomes computationally expensive for long trajectories. For real-time performance or large problems, **iSAM2 (Incremental Smoothing and Mapping)** is the preferred GTSAM algorithm. iSAM2 efficiently updates the solution by only re-linearizing and re-solving parts of the graph affected by new measurements.\n", "* **Accuracy vs. EKF:** This factor graph approach generally handles non-linearities better than a standard EKF because it re-linearizes during optimization. It avoids some of the consistency pitfalls associated with the EKF's single linearization point per step.\n", "* **Visualization:** The Plotly animation shows the evolution of the robot's path estimate, the map of landmarks, and their associated uncertainties (covariance ellipses). You can observe how measurements help refine the estimates and reduce uncertainty, especially when loops are closed (implicitly here through repeated observations of landmarks)." diff --git a/python/gtsam/examples/gtsam_plotly.py b/python/gtsam/examples/gtsam_plotly.py index d37e32dc9..dbbae8f46 100644 --- a/python/gtsam/examples/gtsam_plotly.py +++ b/python/gtsam/examples/gtsam_plotly.py @@ -1,63 +1,78 @@ -# gtsam_plotly_modular_v2.py +# gtsam_plotly_modular_v3.py +import base64 +from dataclasses import dataclass from typing import Any, Callable, Dict, List, Optional, Tuple +import graphviz import numpy as np import plotly.graph_objects as go from tqdm.notebook import tqdm import gtsam -# --- Core Ellipse Calculations --- + +# --- Dataclass for History --- +@dataclass +class SlamFrameData: + """Holds all data needed for a single frame of the SLAM animation.""" + + step_index: int + results: gtsam.Values + marginals: Optional[gtsam.Marginals] + graph_dot_string: Optional[str] = None # Store the Graphviz DOT string -def ellipse_path( - cx: float, cy: float, sizex: float, sizey: float, angle: float, N: int = 60 +# --- Core Ellipse Calculation & Path Generation --- + + +def create_ellipse_path_from_cov( + cx: float, cy: float, cov_matrix: np.ndarray, scale: float = 2.0, N: int = 60 ) -> str: - """Generates SVG path string for a rotated ellipse.""" - angle_rad = np.radians(angle) - t = np.linspace(0, 2 * np.pi, N) - x_unit = (sizex / 2) * np.cos(t) - y_unit = (sizey / 2) * np.sin(t) - x_rot = cx + x_unit * np.cos(angle_rad) - y_unit * np.sin(angle_rad) - y_rot = cy + x_unit * np.sin(angle_rad) + y_unit * np.cos(angle_rad) + """Generates SVG path string for an ellipse directly from 2x2 covariance.""" + cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 # Ensure positive definite + try: + eigvals, eigvecs = np.linalg.eigh(cov) + # eigh sorts eigenvalues in ascending order + eigvals = np.maximum(eigvals, 1e-9) # Ensure positive eigenvalues + minor_std, major_std = np.sqrt(eigvals) + v_minor, v_major = eigvecs[:, 0], eigvecs[:, 1] # Corresponding eigenvectors + except np.linalg.LinAlgError: + # Fallback to a small circle if decomposition fails + radius = 0.1 * scale + t = np.linspace(0, 2 * np.pi, N) + x_p = cx + radius * np.cos(t) + y_p = cy + radius * np.sin(t) + else: + # Parametric equation: Center + scale * major_std * v_major * cos(t) + scale * minor_std * v_minor * sin(t) + # Note: We use scale*std which corresponds to half-axis lengths a,b used before + t = np.linspace(0, 2 * np.pi, N) + cos_t, sin_t = np.cos(t), np.sin(t) + x_p = cx + scale * ( + major_std * cos_t * v_major[0] + minor_std * sin_t * v_minor[0] + ) + y_p = cy + scale * ( + major_std * cos_t * v_major[1] + minor_std * sin_t * v_minor[1] + ) + + # Build SVG path string path = ( - f"M {x_rot[0]},{y_rot[0]} " - + " ".join(f"L{x_},{y_}" for x_, y_ in zip(x_rot[1:], y_rot[1:])) + f"M {x_p[0]},{y_p[0]} " + + " ".join(f"L{x_},{y_}" for x_, y_ in zip(x_p[1:], y_p[1:])) + " Z" ) return path -def gtsam_cov_to_plotly_ellipse( - cov_matrix: np.ndarray, scale: float = 2.0 -) -> Tuple[float, float, float]: - """Calculates ellipse angle (deg), width, height from 2x2 covariance.""" - cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 # Ensure positive definite - try: - eigvals, eigvecs = np.linalg.eigh(cov) - eigvals = np.maximum(eigvals, 1e-9) # Ensure positive eigenvalues - except np.linalg.LinAlgError: - return 0, 0.1 * scale, 0.1 * scale # Default on failure - - width = 2 * scale * np.sqrt(eigvals[1]) # Major axis (largest eigenvalue) - height = 2 * scale * np.sqrt(eigvals[0]) # Minor axis (smallest eigenvalue) - angle_rad = np.arctan2( - eigvecs[1, 1], eigvecs[0, 1] - ) # Angle of major axis eigenvector - angle_deg = np.degrees(angle_rad) - return angle_deg, width, height - - # --- Plotly Element Generators --- -def create_gt_landmarks_trace(landmarks_gt_array: np.ndarray) -> Optional[go.Scatter]: +def create_gt_landmarks_trace(landmarks_gt: np.ndarray) -> Optional[go.Scatter]: """Creates scatter trace for ground truth landmarks.""" - if landmarks_gt_array is None or landmarks_gt_array.size == 0: + if landmarks_gt is None or landmarks_gt.size == 0: return None return go.Scatter( - x=landmarks_gt_array[0, :], - y=landmarks_gt_array[1, :], + x=landmarks_gt[0, :], + y=landmarks_gt[1, :], mode="markers", marker=dict(color="black", size=8, symbol="star"), name="Landmarks GT", @@ -68,11 +83,9 @@ def create_gt_path_trace(poses_gt: List[gtsam.Pose2]) -> Optional[go.Scatter]: """Creates line trace for ground truth path.""" if not poses_gt: return None - gt_path_x = [p.x() for p in poses_gt] - gt_path_y = [p.y() for p in poses_gt] return go.Scatter( - x=gt_path_x, - y=gt_path_y, + x=[p.x() for p in poses_gt], + y=[p.y() for p in poses_gt], mode="lines", line=dict(color="gray", width=1, dash="dash"), name="Path GT", @@ -82,21 +95,21 @@ def create_gt_path_trace(poses_gt: List[gtsam.Pose2]) -> Optional[go.Scatter]: def create_est_path_trace( est_path_x: List[float], est_path_y: List[float] ) -> go.Scatter: - """Creates scatter/line trace for the estimated path up to current step.""" + """Creates trace for the estimated path.""" return go.Scatter( x=est_path_x, y=est_path_y, mode="lines+markers", line=dict(color="red", width=2), marker=dict(size=4, color="red"), - name="Path Est", # This name applies to the trace in the specific frame + name="Path Est", ) def create_est_landmarks_trace( est_landmarks_x: List[float], est_landmarks_y: List[float] ) -> Optional[go.Scatter]: - """Creates scatter trace for currently estimated landmarks.""" + """Creates trace for estimated landmarks.""" if not est_landmarks_x: return None return go.Scatter( @@ -104,136 +117,155 @@ def create_est_landmarks_trace( y=est_landmarks_y, mode="markers", marker=dict(color="blue", size=6, symbol="x"), - name="Landmarks Est", # Applies to landmarks in the specific frame + name="Landmarks Est", ) def _create_ellipse_shape_dict( - cx, cy, angle, width, height, fillcolor, line_color, name_suffix + cx, cy, cov, scale, fillcolor, line_color ) -> Dict[str, Any]: - """Helper to create the dictionary for a Plotly ellipse shape.""" + """Helper: Creates dict for a Plotly ellipse shape from covariance.""" + path = create_ellipse_path_from_cov(cx, cy, cov, scale) return dict( type="path", - path=ellipse_path(cx=cx, cy=cy, sizex=width, sizey=height, angle=angle), + path=path, xref="x", yref="y", fillcolor=fillcolor, line_color=line_color, - # name=f"{name_suffix} Cov", # Name isn't really used by Plotly for shapes ) def create_pose_ellipse_shape( - pose_mean_xy: np.ndarray, pose_cov: np.ndarray, k: int, scale: float + pose_mean_xy: np.ndarray, pose_cov: np.ndarray, scale: float ) -> Dict[str, Any]: - """Creates shape dictionary for a pose covariance ellipse.""" - angle, width, height = gtsam_cov_to_plotly_ellipse(pose_cov, scale) + """Creates shape dict for a pose covariance ellipse.""" return _create_ellipse_shape_dict( cx=pose_mean_xy[0], cy=pose_mean_xy[1], - angle=angle, - width=width, - height=height, + cov=pose_cov, + scale=scale, fillcolor="rgba(255,0,255,0.2)", line_color="rgba(255,0,255,0.5)", - name_suffix=f"Pose {k}", ) def create_landmark_ellipse_shape( - lm_mean_xy: np.ndarray, lm_cov: np.ndarray, lm_index: int, scale: float + lm_mean_xy: np.ndarray, lm_cov: np.ndarray, scale: float ) -> Dict[str, Any]: - """Creates shape dictionary for a landmark covariance ellipse.""" - angle, width, height = gtsam_cov_to_plotly_ellipse(lm_cov, scale) + """Creates shape dict for a landmark covariance ellipse.""" return _create_ellipse_shape_dict( cx=lm_mean_xy[0], cy=lm_mean_xy[1], - angle=angle, - width=width, - height=height, + cov=lm_cov, + scale=scale, fillcolor="rgba(0,0,255,0.1)", line_color="rgba(0,0,255,0.3)", - name_suffix=f"LM {lm_index}", ) +def dot_string_to_base64_png( + dot_string: Optional[str], engine: str = "neato" +) -> Optional[str]: + """Renders a DOT string to a base64 encoded PNG using graphviz.""" + if not dot_string: + return None + source = graphviz.Source(dot_string, engine="neato") + # Use pipe() to get bytes directly without saving a file + png_bytes = source.pipe(format="png") + encoded_string = base64.b64encode(png_bytes).decode() + return f"data:image/png;base64,{encoded_string}" + # --- Frame Content Generation --- - - def generate_frame_content( - k: int, - step_results: gtsam.Values, - step_marginals: Optional[gtsam.Marginals], + frame_data: SlamFrameData, X: Callable[[int], int], L: Callable[[int], int], - max_landmark_index: int, # Need to know the potential range of landmarks + max_landmark_index: int, ellipse_scale: float = 2.0, + graphviz_engine: str = "neato", # Added engine parameter verbose: bool = False, -) -> Tuple[List[go.Scatter], List[Dict[str, Any]]]: - """Generates all dynamic traces and shapes for a single animation frame `k`.""" +) -> Tuple[List[go.Scatter], List[Dict[str, Any]], Optional[Dict[str, Any]]]: + """Generates dynamic traces, shapes, and layout image for a single frame.""" + k = frame_data.step_index + step_results = frame_data.results + step_marginals = frame_data.marginals + frame_traces: List[go.Scatter] = [] frame_shapes: List[Dict[str, Any]] = [] + layout_image: Optional[Dict[str, Any]] = None - # 1. Gather Estimated Path Data - est_path_x = [] - est_path_y = [] - for i in range(k + 1): - pose_key = X(i) - if step_results.exists(pose_key): - pose = step_results.atPose2(pose_key) - est_path_x.append(pose.x()) - est_path_y.append(pose.y()) + # 1. Estimated Path (Unchanged) + est_path_x = [ + step_results.atPose2(X(i)).x() + for i in range(k + 1) + if step_results.exists(X(i)) + ] + est_path_y = [ + step_results.atPose2(X(i)).y() + for i in range(k + 1) + if step_results.exists(X(i)) + ] frame_traces.append(create_est_path_trace(est_path_x, est_path_y)) - # 2. Gather Estimated Landmark Data - est_landmarks_x = [] - est_landmarks_y = [] - landmark_keys_in_frame = [] - # Check all potential landmark keys up to max_landmark_index + # 2. Estimated Landmarks (Unchanged) + est_landmarks_x, est_landmarks_y, landmark_keys = [], [], [] for j in range(max_landmark_index + 1): lm_key = L(j) if step_results.exists(lm_key): lm_point = step_results.atPoint2(lm_key) est_landmarks_x.append(lm_point[0]) est_landmarks_y.append(lm_point[1]) - landmark_keys_in_frame.append(lm_key) - + landmark_keys.append(lm_key) lm_trace = create_est_landmarks_trace(est_landmarks_x, est_landmarks_y) if lm_trace: frame_traces.append(lm_trace) - # 3. Generate Covariance Ellipses (if marginals available) - if step_marginals is not None: - # Pose ellipse - current_pose_key = X(k) - if step_results.exists(current_pose_key): + # 3. Covariance Ellipses (Unchanged) + if step_marginals: + pose_key = X(k) + if step_results.exists(pose_key): try: - pose_cov = step_marginals.marginalCovariance(current_pose_key) - pose_mean = step_results.atPose2(current_pose_key).translation() - frame_shapes.append( - create_pose_ellipse_shape(pose_mean, pose_cov, k, ellipse_scale) - ) + cov = step_marginals.marginalCovariance(pose_key) + mean = step_results.atPose2(pose_key).translation() + frame_shapes.append(create_pose_ellipse_shape(mean, cov, ellipse_scale)) except Exception as e: if verbose: print(f"Warn: Pose {k} cov err @ step {k}: {e}") - - # Landmark ellipses - for lm_key in landmark_keys_in_frame: + for lm_key in landmark_keys: try: - lm_cov = step_marginals.marginalCovariance(lm_key) - lm_mean = step_results.atPoint2(lm_key) - lm_index = gtsam.Symbol(lm_key).index() + cov = step_marginals.marginalCovariance(lm_key) + mean = step_results.atPoint2(lm_key) frame_shapes.append( - create_landmark_ellipse_shape( - lm_mean, lm_cov, lm_index, ellipse_scale - ) + create_landmark_ellipse_shape(mean, cov, ellipse_scale) ) except Exception as e: - lm_index = gtsam.Symbol(lm_key).index() if verbose: - print(f"Warn: LM {lm_index} cov err @ step {k}: {e}") + print( + f"Warn: LM {gtsam.Symbol(lm_key).index()} cov err @ step {k}: {e}" + ) - return frame_traces, frame_shapes + # 4. Graph Image for Layout (MODIFIED) + # Use the new function with the dot string from frame_data + img_src = dot_string_to_base64_png( + frame_data.graph_dot_string, engine=graphviz_engine + ) + if img_src: + layout_image = dict( + source=img_src, + xref="paper", + yref="paper", + x=0, + y=1, + sizex=0.48, + sizey=1, + xanchor="left", + yanchor="top", + layer="below", + sizing="contain", + ) + + return frame_traces, frame_shapes, layout_image # --- Figure Configuration --- @@ -244,9 +276,12 @@ def configure_figure_layout( num_steps: int, world_size: float, initial_shapes: List[Dict[str, Any]], + initial_image: Optional[Dict[str, Any]], ) -> None: - """Configures Plotly figure layout, axes, slider, buttons.""" + """Configures Plotly figure layout for side-by-side display.""" steps = list(range(num_steps + 1)) + plot_domain = [0.52, 1.0] # Right pane for the SLAM plot + sliders = [ dict( active=0, @@ -275,10 +310,10 @@ def configure_figure_layout( showactive=False, direction="left", pad={"r": 10, "t": 87}, - x=0.1, - xanchor="right", + x=plot_domain[0], + xanchor="left", y=0, - yanchor="top", + yanchor="top", # Position relative to plot area buttons=[ dict( label="Play", @@ -310,28 +345,29 @@ def configure_figure_layout( ] fig.update_layout( - title="Iterative Factor Graph SLAM Animation", - xaxis=dict(range=[-world_size / 2 - 2, world_size / 2 + 2], constrain="domain"), + title="Factor Graph SLAM Animation (Graph Left, Results Right)", + xaxis=dict( + range=[-world_size / 2 - 2, world_size / 2 + 2], + domain=plot_domain, # Confine axis to right pane + constrain="domain", + ), yaxis=dict( range=[-world_size / 2 - 2, world_size / 2 + 2], scaleanchor="x", scaleratio=1, + domain=[0, 1], # Full height within its domain ), - width=800, - height=800, + width=1000, + height=600, # Adjust size for side-by-side hovermode="closest", updatemenus=updatemenus, sliders=sliders, - shapes=initial_shapes, - legend=dict( - traceorder="reversed", - title_text="Legend", - orientation="h", - yanchor="bottom", - y=1.02, - xanchor="right", - x=1, - ), + shapes=initial_shapes, # Initial shapes for the SLAM plot + images=( + [initial_image] if initial_image else [] + ), # Initial image for the left pane + showlegend=False, # Forego legend for space + # Autosize=True might help responsiveness but can interfere with domain ) @@ -339,23 +375,25 @@ def configure_figure_layout( def create_slam_animation( - results_history: List[gtsam.Values], - marginals_history: List[Optional[gtsam.Marginals]], - num_steps: int, + history: List[SlamFrameData], X: Callable[[int], int], L: Callable[[int], int], - max_landmark_index: int, # Required to iterate potential landmarks + max_landmark_index: int, landmarks_gt_array: Optional[np.ndarray] = None, poses_gt: Optional[List[gtsam.Pose2]] = None, world_size: float = 20.0, ellipse_scale: float = 2.0, + graphviz_engine: str = "neato", # Pass engine choice verbose_cov_errors: bool = False, ) -> go.Figure: - """Creates a modular Plotly SLAM animation.""" + """Creates a side-by-side Plotly SLAM animation using a history of dataclasses.""" + if not history: + raise ValueError("History cannot be empty.") print("Generating Plotly animation...") + num_steps = history[-1].step_index fig = go.Figure() - # 1. Add static ground truth traces to the base figure (visible always) + # 1. Add static GT traces (Unchanged) gt_lm_trace = create_gt_landmarks_trace(landmarks_gt_array) if gt_lm_trace: fig.add_trace(gt_lm_trace) @@ -363,51 +401,58 @@ def create_slam_animation( if gt_path_trace: fig.add_trace(gt_path_trace) - # 2. Generate frames with dynamic content + # 2. Generate frames (MODIFIED: pass graphviz_engine) frames = [] + initial_dynamic_traces, initial_shapes, initial_image = [], [], None steps_iterable = range(num_steps + 1) try: steps_iterable = tqdm(steps_iterable, desc="Creating Frames") except NameError: - pass # tqdm optional + pass for k in steps_iterable: - step_results = results_history[k] - step_marginals = marginals_history[k] if marginals_history else None + frame_data = next((item for item in history if item.step_index == k), None) + if frame_data is None: + print(f"Warning: Missing data for step {k} in history.") + continue - frame_traces, frame_shapes = generate_frame_content( - k, - step_results, - step_marginals, + # Pass the engine choice to the content generator + frame_traces, frame_shapes, layout_image = generate_frame_content( + frame_data, X, L, max_landmark_index, ellipse_scale, + graphviz_engine, verbose_cov_errors, ) + + if k == 0: + initial_dynamic_traces, initial_shapes, initial_image = ( + frame_traces, + frame_shapes, + layout_image, + ) + frames.append( go.Frame( - data=frame_traces, name=str(k), layout=go.Layout(shapes=frame_shapes) + data=frame_traces, + name=str(k), + layout=go.Layout( + shapes=frame_shapes, images=[layout_image] if layout_image else [] + ), ) ) - # 3. Set initial dynamic data (from frame 0) onto the base figure - initial_dynamic_traces = [] - initial_shapes = [] - if frames: - # Important: Add *copies* or ensure traces are regenerated if needed, - # though Plotly usually handles this ok with frame data. - initial_dynamic_traces = frames[0].data - initial_shapes = frames[0].layout.shapes if frames[0].layout else [] - for trace in initial_dynamic_traces: - fig.add_trace(trace) # Add Est Path[0], Est Landmarks[0] traces + # 3. Add initial dynamic traces (Unchanged) + for trace in initial_dynamic_traces: + fig.add_trace(trace) - # 4. Assign frames to the figure + # 4. Assign frames (Unchanged) fig.update(frames=frames) - # 5. Configure layout, axes, controls - # Pass initial_shapes for the layout's starting state - configure_figure_layout(fig, num_steps, world_size, initial_shapes) + # 5. Configure layout (Unchanged) + configure_figure_layout(fig, num_steps, world_size, initial_shapes, initial_image) print("Plotly animation generated.") return fig From d1780601cd44498f2acb0b3e8add64a2d95584c4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 25 Apr 2025 09:50:41 -0400 Subject: [PATCH 8/9] SVG graphs --- python/gtsam/examples/EKF_SLAM.ipynb | 9 +- python/gtsam/examples/gtsam_plotly.py | 142 +++++++++++++++++--------- python/gtsam/examples/simulation.py | 2 +- 3 files changed, 99 insertions(+), 54 deletions(-) diff --git a/python/gtsam/examples/EKF_SLAM.ipynb b/python/gtsam/examples/EKF_SLAM.ipynb index 265005710..48a262f5e 100644 --- a/python/gtsam/examples/EKF_SLAM.ipynb +++ b/python/gtsam/examples/EKF_SLAM.ipynb @@ -108,17 +108,12 @@ "import numpy as np\n", "from tqdm.notebook import tqdm # Progress bar\n", "import math\n", - "import time # To slow down graphviz display if needed\n", "\n", "import gtsam\n", "from gtsam.symbol_shorthand import X, L # Symbols for poses and landmarks\n", "\n", - "# Imports for new modules\n", "import simulation\n", - "from gtsam_plotly import SlamFrameData, create_slam_animation\n", - "\n", - "# Imports for graph visualization\n", - "import graphviz" + "from gtsam_plotly import SlamFrameData, create_slam_animation" ] }, { @@ -143,7 +138,7 @@ "# Robot parameters\n", "ROBOT_RADIUS = 3.0\n", "ROBOT_ANGULAR_VEL = np.deg2rad(20.0) # Radians per step\n", - "NUM_STEPS = 20 # Reduced steps for faster animation generation\n", + "NUM_STEPS = 50 # Reduced steps for faster animation generation\n", "DT = 1.0 # Time step duration (simplified)\n", "\n", "# Noise parameters (GTSAM Noise Models)\n", diff --git a/python/gtsam/examples/gtsam_plotly.py b/python/gtsam/examples/gtsam_plotly.py index dbbae8f46..a0d6e0ed3 100644 --- a/python/gtsam/examples/gtsam_plotly.py +++ b/python/gtsam/examples/gtsam_plotly.py @@ -164,17 +164,30 @@ def create_landmark_ellipse_shape( ) -def dot_string_to_base64_png( +def dot_string_to_base64_svg( dot_string: Optional[str], engine: str = "neato" ) -> Optional[str]: - """Renders a DOT string to a base64 encoded PNG using graphviz.""" + """Renders a DOT string to a base64 encoded SVG using graphviz.""" if not dot_string: return None - source = graphviz.Source(dot_string, engine="neato") - # Use pipe() to get bytes directly without saving a file - png_bytes = source.pipe(format="png") - encoded_string = base64.b64encode(png_bytes).decode() - return f"data:image/png;base64,{encoded_string}" + try: + # Set DPI for potentially better default sizing, though less critical for SVG + # graph_attr = {'dpi': '150'} # You can experiment with this + source = graphviz.Source(dot_string, engine=engine) # , graph_attr=graph_attr) + # Use pipe() to get bytes directly without saving a file, format="svg" + svg_bytes = source.pipe(format="svg") + # Encode bytes to base64 string, decode result to UTF-8 string + encoded_string = base64.b64encode(svg_bytes).decode("utf-8") + # Return data URI for SVG + return f"data:image/svg+xml;base64,{encoded_string}" + except graphviz.backend.execute.CalledProcessError as e: + print(f"Graphviz rendering error ({engine}): {e}") + # Optionally return a placeholder or raise + return None + except Exception as e: + print(f"Unexpected error during Graphviz SVG generation: {e}") + return None + # --- Frame Content Generation --- def generate_frame_content( @@ -183,7 +196,7 @@ def generate_frame_content( L: Callable[[int], int], max_landmark_index: int, ellipse_scale: float = 2.0, - graphviz_engine: str = "neato", # Added engine parameter + graphviz_engine: str = "neato", verbose: bool = False, ) -> Tuple[List[go.Scatter], List[Dict[str, Any]], Optional[Dict[str, Any]]]: """Generates dynamic traces, shapes, and layout image for a single frame.""" @@ -191,11 +204,11 @@ def generate_frame_content( step_results = frame_data.results step_marginals = frame_data.marginals - frame_traces: List[go.Scatter] = [] + frame_dynamic_traces: List[go.Scatter] = [] # Renamed for clarity frame_shapes: List[Dict[str, Any]] = [] layout_image: Optional[Dict[str, Any]] = None - # 1. Estimated Path (Unchanged) + # 1. Estimated Path est_path_x = [ step_results.atPose2(X(i)).x() for i in range(k + 1) @@ -206,9 +219,9 @@ def generate_frame_content( for i in range(k + 1) if step_results.exists(X(i)) ] - frame_traces.append(create_est_path_trace(est_path_x, est_path_y)) + frame_dynamic_traces.append(create_est_path_trace(est_path_x, est_path_y)) - # 2. Estimated Landmarks (Unchanged) + # 2. Estimated Landmarks est_landmarks_x, est_landmarks_y, landmark_keys = [], [], [] for j in range(max_landmark_index + 1): lm_key = L(j) @@ -219,9 +232,9 @@ def generate_frame_content( landmark_keys.append(lm_key) lm_trace = create_est_landmarks_trace(est_landmarks_x, est_landmarks_y) if lm_trace: - frame_traces.append(lm_trace) + frame_dynamic_traces.append(lm_trace) - # 3. Covariance Ellipses (Unchanged) + # 3. Covariance Ellipses if step_marginals: pose_key = X(k) if step_results.exists(pose_key): @@ -229,9 +242,13 @@ def generate_frame_content( cov = step_marginals.marginalCovariance(pose_key) mean = step_results.atPose2(pose_key).translation() frame_shapes.append(create_pose_ellipse_shape(mean, cov, ellipse_scale)) - except Exception as e: + except RuntimeError as e: # More specific exception type if verbose: print(f"Warn: Pose {k} cov err @ step {k}: {e}") + except Exception as e: + if verbose: + print(f"Warn: Pose {k} cov OTHER err @ step {k}: {e}") + for lm_key in landmark_keys: try: cov = step_marginals.marginalCovariance(lm_key) @@ -239,15 +256,20 @@ def generate_frame_content( frame_shapes.append( create_landmark_ellipse_shape(mean, cov, ellipse_scale) ) - except Exception as e: + except RuntimeError as e: # More specific exception type if verbose: print( f"Warn: LM {gtsam.Symbol(lm_key).index()} cov err @ step {k}: {e}" ) + except Exception as e: + if verbose: + print( + f"Warn: LM {gtsam.Symbol(lm_key).index()} cov OTHER err @ step {k}: {e}" + ) - # 4. Graph Image for Layout (MODIFIED) - # Use the new function with the dot string from frame_data - img_src = dot_string_to_base64_png( + # 4. Graph Image for Layout (MODIFIED TO USE SVG) + # Use the SVG function with the dot string from frame_data + img_src = dot_string_to_base64_svg( # Changed function call frame_data.graph_dot_string, engine=graphviz_engine ) if img_src: @@ -262,10 +284,11 @@ def generate_frame_content( xanchor="left", yanchor="top", layer="below", - sizing="contain", + sizing="contain", # Contain should work well with SVG ) - return frame_traces, frame_shapes, layout_image + # Return only the things that CHANGE frame-to-frame + return frame_dynamic_traces, frame_shapes, layout_image # --- Figure Configuration --- @@ -374,6 +397,9 @@ def configure_figure_layout( # --- Main Animation Orchestrator --- +# --- Main Animation Orchestrator (MODIFIED TO FIX GT DISPLAY) --- + + def create_slam_animation( history: List[SlamFrameData], X: Callable[[int], int], @@ -383,7 +409,7 @@ def create_slam_animation( poses_gt: Optional[List[gtsam.Pose2]] = None, world_size: float = 20.0, ellipse_scale: float = 2.0, - graphviz_engine: str = "neato", # Pass engine choice + graphviz_engine: str = "neato", verbose_cov_errors: bool = False, ) -> go.Figure: """Creates a side-by-side Plotly SLAM animation using a history of dataclasses.""" @@ -393,31 +419,63 @@ def create_slam_animation( num_steps = history[-1].step_index fig = go.Figure() - # 1. Add static GT traces (Unchanged) + # 1. Create static GT traces ONCE + gt_traces = [] gt_lm_trace = create_gt_landmarks_trace(landmarks_gt_array) if gt_lm_trace: - fig.add_trace(gt_lm_trace) + gt_traces.append(gt_lm_trace) gt_path_trace = create_gt_path_trace(poses_gt) if gt_path_trace: - fig.add_trace(gt_path_trace) + gt_traces.append(gt_path_trace) - # 2. Generate frames (MODIFIED: pass graphviz_engine) + # 2. Generate content for the initial frame (k=0) to set up the figure + initial_frame_data = next((item for item in history if item.step_index == 0), None) + if initial_frame_data is None: + raise ValueError("History must contain data for step 0.") + + ( + initial_dynamic_traces, + initial_shapes, + initial_image, + ) = generate_frame_content( + initial_frame_data, + X, + L, + max_landmark_index, + ellipse_scale, + graphviz_engine, + verbose_cov_errors, + ) + + # 3. Add initial traces to the figure (GT first, then initial dynamic traces) + # These define the plot state BEFORE the animation starts or when slider is at 0 + for trace in gt_traces: + fig.add_trace(trace) + for trace in initial_dynamic_traces: + fig.add_trace(trace) + + # 4. Generate frames for the animation (k=0 to num_steps) frames = [] - initial_dynamic_traces, initial_shapes, initial_image = [], [], None steps_iterable = range(num_steps + 1) try: steps_iterable = tqdm(steps_iterable, desc="Creating Frames") - except NameError: + except NameError: # Handle if tqdm is not available (e.g. standard Python script) pass + except Exception as e: # Catch other potential tqdm issues + print( + f"Note: Could not initialize tqdm progress bar ({e}). Continuing without it." + ) for k in steps_iterable: frame_data = next((item for item in history if item.step_index == k), None) if frame_data is None: - print(f"Warning: Missing data for step {k} in history.") + print(f"Warning: Missing data for step {k} in history. Skipping frame.") + # Optionally create an empty frame or reuse previous frame's data + # For simplicity, we skip here, which might cause jumps in animation continue - # Pass the engine choice to the content generator - frame_traces, frame_shapes, layout_image = generate_frame_content( + # Generate dynamic content for this specific frame + frame_dynamic_traces, frame_shapes, layout_image = generate_frame_content( frame_data, X, L, @@ -427,31 +485,23 @@ def create_slam_animation( verbose_cov_errors, ) - if k == 0: - initial_dynamic_traces, initial_shapes, initial_image = ( - frame_traces, - frame_shapes, - layout_image, - ) - + # IMPORTANT: Combine static GT traces with the dynamic traces for THIS frame + # The 'data' in the frame replaces the figure's data for that step. frames.append( go.Frame( - data=frame_traces, + data=gt_traces + frame_dynamic_traces, # Include GT in every frame name=str(k), - layout=go.Layout( + layout=go.Layout( # Only update shapes and images in layout per frame shapes=frame_shapes, images=[layout_image] if layout_image else [] ), ) ) - # 3. Add initial dynamic traces (Unchanged) - for trace in initial_dynamic_traces: - fig.add_trace(trace) - - # 4. Assign frames (Unchanged) + # 5. Assign frames to the figure fig.update(frames=frames) - # 5. Configure layout (Unchanged) + # 6. Configure overall layout (sliders, buttons, axes, etc.) + # Pass the shapes and image corresponding to the INITIAL state (k=0) configure_figure_layout(fig, num_steps, world_size, initial_shapes, initial_image) print("Plotly animation generated.") diff --git a/python/gtsam/examples/simulation.py b/python/gtsam/examples/simulation.py index a36433878..66beaa1ba 100644 --- a/python/gtsam/examples/simulation.py +++ b/python/gtsam/examples/simulation.py @@ -103,7 +103,7 @@ def generate_simulation_data( # Check sensor limits (range and Field of View - e.g. +/- 45 degrees) if ( true_range <= max_sensor_range - and abs(true_bearing.theta()) < np.pi / 4 + and abs(true_bearing.theta()) < np.pi / 2 ): # Sample noise noise_vec = measurement_noise_sampler.sample() From b07b77c40d4f1384e83dcf8a80f74167703d5a47 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 09:43:05 -0400 Subject: [PATCH 9/9] Better plotting, true filter --- gtsam/nonlinear/nonlinear.i | 5 +- gtsam/symbolic/symbolic.md | 2 +- python/gtsam/examples/EKF_SLAM.ipynb | 16354 +++++++++++++++++++++++- python/gtsam/examples/gtsam_plotly.py | 209 +- 4 files changed, 16313 insertions(+), 257 deletions(-) diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index c13defbad..adde6d91d 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -669,17 +669,17 @@ virtual class NonlinearEquality2 : gtsam::NoiseModelFactor { }; #include +// This class is not available in python, just use a dictionary class FixedLagSmootherKeyTimestampMapValue { FixedLagSmootherKeyTimestampMapValue(size_t key, double timestamp); FixedLagSmootherKeyTimestampMapValue(const gtsam::FixedLagSmootherKeyTimestampMapValue& other); }; +// This class is not available in python, just use a dictionary class FixedLagSmootherKeyTimestampMap { FixedLagSmootherKeyTimestampMap(); FixedLagSmootherKeyTimestampMap(const gtsam::FixedLagSmootherKeyTimestampMap& other); - // Note: no print function - // common STL methods size_t size() const; bool empty() const; @@ -739,6 +739,7 @@ virtual class IncrementalFixedLagSmoother : gtsam::FixedLagSmoother { void print(string s = "IncrementalFixedLagSmoother:\n") const; + gtsam::Matrix marginalCovariance(size_t key) const; gtsam::ISAM2Params params() const; gtsam::NonlinearFactorGraph getFactors() const; diff --git a/gtsam/symbolic/symbolic.md b/gtsam/symbolic/symbolic.md index bfae60842..cc2f78d31 100644 --- a/gtsam/symbolic/symbolic.md +++ b/gtsam/symbolic/symbolic.md @@ -1,4 +1,4 @@ -# Symbolic Module +# Symbolic The `symbolic` module in GTSAM deals with the *structure* of factor graphs and Bayesian networks, independent of the specific numerical types of factors (like Gaussian or discrete). It allows for analyzing graph connectivity, determining optimal variable elimination orders, and understanding the sparsity structure of the resulting inference objects. diff --git a/python/gtsam/examples/EKF_SLAM.ipynb b/python/gtsam/examples/EKF_SLAM.ipynb index 48a262f5e..fc3642728 100644 --- a/python/gtsam/examples/EKF_SLAM.ipynb +++ b/python/gtsam/examples/EKF_SLAM.ipynb @@ -2,15 +2,15 @@ "cells": [ { "cell_type": "markdown", - "id": "27cd96fc", + "id": "title-cell-fls", "metadata": {}, "source": [ - "# EKF-SLAM using Factor Graphs (SRIF-like)" + "# EKF-SLAM (using Incremental Fixed-Lag Smoother)" ] }, { "cell_type": "markdown", - "id": "copyright-cell-srif", + "id": "copyright-cell-fls", "metadata": { "tags": [ "remove-cell" @@ -27,7 +27,7 @@ }, { "cell_type": "markdown", - "id": "colab-button-cell-srif", + "id": "colab-button-cell-fls", "metadata": {}, "source": [ "\"Open" @@ -35,49 +35,19 @@ }, { "cell_type": "markdown", - "id": "intro-srif", + "id": "intro-fls-short", "metadata": {}, "source": [ - "This notebook demonstrates 2D SLAM using GTSAM's factor graph tools in an **iterative, filter-like manner**, similar in structure to a Square Root Information Filter (SRIF) or an EKF, but leveraging factor graph elimination instead of explicit matrix operations.\n", + "This notebook demonstrates 2D Simultaneous Localization and Mapping (SLAM) using an EKF, although it is implemented using GTSAM's `IncrementalFixedLagSmoother`, just using a lag of 1.\n", "\n", - "**Scenario:**\n", - "A robot moves in a circular path within a 10x10 meter area containing randomly placed landmarks. It receives noisy odometry and bearing-range measurements.\n", + "**Scenario:** A robot moves in a circular path, receiving noisy odometry and bearing-range measurements to landmarks.\n", "\n", - "**Factor Graph Filtering Approach:**\n", - "Instead of maintaining a large state vector and covariance matrix explicitly (like in classic EKF), we use GTSAM's `NonlinearFactorGraph` and `GaussianFactorGraph`:\n", - "\n", - "1. **Belief Representation:** The robot's belief (estimated state and uncertainty) after each step is implicitly represented by a factor graph (or the resulting Bayes Net after elimination).\n", - "2. **Prediction:**\n", - " * Start with the factors representing the belief from the *previous* step.\n", - " * Add a new `BetweenFactorPose2` representing the noisy odometry measurement, connecting the previous pose `X(k)` to the new pose `X(k+1)`.\n", - " * Linearize all relevant factors around the current best estimate (`Values`).\n", - " * Eliminate the resulting `GaussianFactorGraph` using an ordering that marginalizes out the previous pose `X(k)`, yielding a `GaussianBayesNet` representing the *predictive* distribution over landmarks and `X(k+1)`.\n", - "3. **Update:**\n", - " * Start with the factors representing the *predictive* belief.\n", - " * For each landmark measurement at the new pose `X(k+1)`:\n", - " * If the landmark `L(j)` is new, add it to the `Values` (initial estimate).\n", - " * Add a `BearingRangeFactor2D` connecting `X(k+1)` and `L(j)`.\n", - " * Linearize new/updated factors.\n", - " * Eliminate the `GaussianFactorGraph` to incorporate the measurement information, yielding a `GaussianBayesNet` representing the *posterior* belief.\n", - " * Optimize the final Bayes Net to get the updated state estimate (`Values`).\n", - "4. **Iteration:** Repeat prediction and update for each time step.\n", - "\n", - "**Enhancements in this version:**\n", - "* Simulation code is moved to `simulation.py`.\n", - "* Plotting code is moved to `gtsam_plotly.py`.\n", - "* The evolving factor graph is displayed at each step using `graphviz`.\n", - "\n", - "**Advantages:**\n", - "* Leverages GTSAM's robust factor graph machinery.\n", - "* Handles non-linearities through iterative linearization (like EKF, but potentially more robust within GTSAM's optimization context).\n", - "* Avoids explicit management of large, dense covariance matrices.\n", - "\n", - "**Output:** The process will be visualized with the evolving factor graph, and finally using a Plotly animation showing the robot's estimated path, mapped landmarks, and uncertainty ellipses evolving over time." + "**Approach:** We use a fixed-lag smoother which maintains and optimizes only a recent window of variables (defined by the `SMOOTHER_LAG`). Variables older than the lag are marginalized out, keeping the computational cost bounded, making it suitable for online applications. By default we set the lag to *1* here, which makes this an extended Kalman filter. But **feel free to change the lag and see the fixed-lag smoother results**." ] }, { "cell_type": "markdown", - "id": "setup-imports-srif", + "id": "setup-imports-fls", "metadata": {}, "source": [ "## 1. Setup and Imports" @@ -85,8 +55,8 @@ }, { "cell_type": "code", - "execution_count": null, - "id": "install-code-srif", + "execution_count": 1, + "id": "install-code-fls", "metadata": {}, "outputs": [], "source": [ @@ -100,8 +70,8 @@ }, { "cell_type": "code", - "execution_count": null, - "id": "imports-code-srif", + "execution_count": 2, + "id": "imports-code-fls", "metadata": {}, "outputs": [], "source": [ @@ -112,22 +82,28 @@ "import gtsam\n", "from gtsam.symbol_shorthand import X, L # Symbols for poses and landmarks\n", "\n", + "# Import the IncrementalFixedLagSmoother\n", + "from gtsam import IncrementalFixedLagSmoother\n", + "\n", + "# Helper modules\n", "import simulation\n", "from gtsam_plotly import SlamFrameData, create_slam_animation" ] }, { "cell_type": "markdown", - "id": "params-srif", + "id": "params-fls", "metadata": {}, "source": [ - "## 2. Simulation Parameters" + "## 2. Simulation and Smoother Parameters\n", + "\n", + "Define parameters for the simulation environment, robot motion, noise models, and the fixed-lag smoother." ] }, { "cell_type": "code", - "execution_count": null, - "id": "params-code-srif", + "execution_count": 9, + "id": "params-code-fls", "metadata": {}, "outputs": [], "source": [ @@ -138,37 +114,51 @@ "# Robot parameters\n", "ROBOT_RADIUS = 3.0\n", "ROBOT_ANGULAR_VEL = np.deg2rad(20.0) # Radians per step\n", - "NUM_STEPS = 50 # Reduced steps for faster animation generation\n", - "DT = 1.0 # Time step duration (simplified)\n", + "NUM_STEPS = 50\n", + "DT = 1.0 # Time step duration\n", "\n", "# Noise parameters (GTSAM Noise Models)\n", - "# Prior noise on the first pose (x, y, theta)\n", "PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, np.deg2rad(1.0)]))\n", - "# Odometry noise (dx, dy, dtheta)\n", "ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.05, np.deg2rad(2.0)]))\n", - "# Measurement noise (bearing, range)\n", "MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([np.deg2rad(2.0), 0.2]))\n", "\n", "# Sensor parameters\n", - "MAX_SENSOR_RANGE = 5.0" + "MAX_SENSOR_RANGE = 5.0\n", + "\n", + "# --- Fixed-Lag Smoother Parameters ---\n", + "# Define the length of the smoothing window in seconds.\n", + "# A lag of 1*DT (e.g., 1.0 second) means the smoother maintains the state\n", + "# estimate for the current time step only, behaving like a filter.\n", + "# Larger lags incorporate more past information for smoothing.\n", + "SMOOTHER_LAG = 0.99 * DT\n" ] }, { "cell_type": "markdown", - "id": "ground-truth-srif", + "id": "ground-truth-fls", "metadata": {}, "source": [ "## 3. Generate Ground Truth Data\n", "\n", - "Use the `simulation` module to create true landmark positions, robot path, and simulate noisy measurements." + "Create the true environment, robot path, and simulate noisy sensor readings using the `simulation` module." ] }, { "cell_type": "code", - "execution_count": null, - "id": "ground-truth-call-srif", + "execution_count": 10, + "id": "ground-truth-call-fls", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Simulation Generated: 15 landmarks.\n", + "Simulation Generated: 51 ground truth poses and 50 odometry measurements.\n", + "Simulation Generated: 210 bearing-range measurements.\n" + ] + } + ], "source": [ "landmarks_gt_dict, poses_gt, odometry_measurements, measurements_sim, landmarks_gt_array = \\\n", " simulation.generate_simulation_data(\n", @@ -188,159 +178,16215 @@ }, { "cell_type": "markdown", - "id": "factor-graph-slam-impl", + "id": "smoother-impl", "metadata": {}, "source": [ - "## 4. Iterative Factor Graph SLAM Implementation\n", + "## 4. Fixed-Lag Smoother SLAM Implementation" + ] + }, + { + "cell_type": "markdown", + "id": "smoother-init", + "metadata": {}, + "source": [ + "### Initialize Smoother and Helper Functions\n", "\n", - "Here we perform the step-by-step prediction and update using factor graphs. The evolving graph will be displayed." + "We create the `IncrementalFixedLagSmoother` with the specified lag. We also initialize the first state (pose X(0) at time 0.0) and add it to the smoother using its `update` method. The `update` method requires factors, initial values (theta), and timestamps for the *new* variables being added." ] }, { "cell_type": "code", - "execution_count": null, - "id": "90bf9320", + "execution_count": 11, + "id": "smoother-init-code", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Initializing IncrementalFixedLagSmoother with lag = 0.99 seconds...\n", + "Performing initial smoother update...\n", + "Initial update complete.\n" + ] + } + ], "source": [ + "# Helper for graphviz visualization\n", "WRITER = gtsam.GraphvizFormatting()\n", "WRITER.binaryEdges = True\n", "\n", - "def mage_dot(graph, estimate):\n", - " return graph.dot(estimate, writer=WRITER)\n" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "id": "factor-graph-init-code", - "metadata": {}, - "outputs": [], - "source": [ - "# --- Initialization ---\n", - "current_estimate = gtsam.Values() # Stores current best estimates (linearization points)\n", - "current_graph = gtsam.NonlinearFactorGraph() # Stores factors added so far\n", - "current_pose_key = X(0)\n", + "def make_dot(graph, estimate):\n", + " # Visualize the factor graph currently managed by the smoother\n", + " WRITER.boxes = {key for key in estimate.keys() if gtsam.symbolChr(key) == ord('l')}\n", + " return graph.dot(estimate, writer=WRITER)\n", "\n", - "# Add prior on the initial pose X(0)\n", - "initial_pose = poses_gt[0] # Start at ground truth (or add noise)\n", - "# Add a bit of noise to the initial guess if desired:\n", - "# initial_pose_noisy = initial_pose.compose(gtsam.Pose2(0.1, 0.1, 0.01))\n", - "# current_estimate.insert(current_pose_key, initial_pose_noisy)\n", - "current_estimate.insert(current_pose_key, initial_pose) \n", - "\n", - "current_graph.add(gtsam.PriorFactorPose2(current_pose_key, initial_pose, PRIOR_NOISE))\n", + "# --- Smoother Initialization ---\n", + "print(f\"Initializing IncrementalFixedLagSmoother with lag = {SMOOTHER_LAG} seconds...\")\n", + "# Use ISAM2 parameters if specific settings are needed, otherwise defaults are used.\n", + "isam2_params = gtsam.ISAM2Params()\n", + "smoother = IncrementalFixedLagSmoother(SMOOTHER_LAG, isam2_params)\n", "\n", "# Variables to store results for animation\n", - "history = [] # Store SLAMFrameData objects for each step\n", - "known_landmarks = set() # Set of landmark keys L(j) currently in the state\n", + "history = []\n", "\n", - "# Initial marginals (only for X(0))\n", - "initial_gaussian_graph = current_graph.linearize(current_estimate)\n", - "initial_marginals = gtsam.Marginals(initial_gaussian_graph, current_estimate)\n", - "history.append(SlamFrameData(0, current_estimate, initial_marginals, mage_dot(current_graph, current_estimate)))" + "# --- Initial Step (k=0) ---\n", + "initial_pose_key = X(0)\n", + "initial_time = 0.0\n", + "initial_pose = poses_gt[0] # Start at ground truth (can add noise if desired)\n", + "\n", + "# Create containers for the first update\n", + "initial_factors = gtsam.NonlinearFactorGraph()\n", + "initial_values = gtsam.Values()\n", + "# The KeyTimestampMap maps variable keys (size_t) to their timestamps (double)\n", + "initial_timestamps = {}\n", + "\n", + "# Add prior factor for the first pose\n", + "initial_factors.add(gtsam.PriorFactorPose2(initial_pose_key, initial_pose, PRIOR_NOISE))\n", + "\n", + "# Add the initial pose estimate to Values\n", + "initial_values.insert(initial_pose_key, initial_pose)\n", + "\n", + "# Add the timestamp for the initial pose\n", + "initial_timestamps[initial_pose_key] = initial_time\n", + "\n", + "# Update the smoother with the initial state\n", + "print(\"Performing initial smoother update...\")\n", + "smoother.update(initial_factors, initial_values, initial_timestamps)\n", + "print(\"Initial update complete.\")\n", + "\n", + "# Store initial state for animation\n", + "current_estimate = smoother.calculateEstimate()\n", + "current_graph = smoother.getFactors() # Get factors currently managed by smoother\n", + "# ISAM can serve as marginals object:\n", + "current_marginals = smoother.getISAM2()\n", + "\n", + "history.append(SlamFrameData(0, current_estimate, current_marginals, make_dot(current_graph, current_estimate)))" ] }, { "cell_type": "markdown", - "id": "factor-graph-loop", + "id": "smoother-loop", "metadata": {}, "source": [ - "### Main Iterative Loop" + "### Main Iterative Loop\n", + "\n", + "At each step `k`, we process the odometry measurement from `X(k)` to `X(k+1)` and all landmark measurements taken *at* pose `X(k+1)`.\n", + "\n", + "1. **Prepare Data:** Collect new factors (`NonlinearFactorGraph`), initial estimates for *new* variables (`Values`), and timestamps for *new* variables (`KeyTimestampMap`) for the current step.\n", + "2. **Predict:** Calculate an initial estimate for the new pose `X(k+1)` based on the previous estimate `X(k)` (retrieved from the smoother) and the odometry measurement.\n", + "3. **Initialize Landmarks:** If a landmark is observed for the first time, calculate an initial estimate based on the predicted pose and the measurement, and add it to the `new_values` and `new_timestamps`.\n", + "4. **Update Smoother:** Call `smoother.update()` with the collected factors, values, and timestamps. This incorporates the new information, performs optimization (iSAM2), and marginalizes old variables.\n", + "5. **Store Results:** Retrieve the current estimate and factor graph from the smoother for visualization." ] }, { "cell_type": "code", - "execution_count": null, - "id": "factor-graph-loop-code", + "execution_count": 12, + "id": "smoother-loop-code", "metadata": {}, - "outputs": [], + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Running Incremental Fixed-Lag Smoother SLAM loop (50 steps)...\n" + ] + }, + { + "data": { + "application/vnd.jupyter.widget-view+json": { + "model_id": "65c6048ecb324a96ac367007900bb015", + "version_major": 2, + "version_minor": 0 + }, + "text/plain": [ + " 0%| | 0/50 [00:00 str: - """Generates SVG path string for an ellipse directly from 2x2 covariance.""" - cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 # Ensure positive definite + """Generates SVG path string for an ellipse from 2x2 covariance.""" + cov = cov_matrix[:2, :2] + np.eye(2) * 1e-9 # Ensure positive definite 2x2 try: eigvals, eigvecs = np.linalg.eigh(cov) - # eigh sorts eigenvalues in ascending order eigvals = np.maximum(eigvals, 1e-9) # Ensure positive eigenvalues - minor_std, major_std = np.sqrt(eigvals) - v_minor, v_major = eigvecs[:, 0], eigvecs[:, 1] # Corresponding eigenvectors + minor_std, major_std = np.sqrt(eigvals) # eigh sorts ascending + v_minor, v_major = eigvecs[:, 0], eigvecs[:, 1] except np.linalg.LinAlgError: # Fallback to a small circle if decomposition fails radius = 0.1 * scale @@ -43,8 +42,7 @@ def create_ellipse_path_from_cov( x_p = cx + radius * np.cos(t) y_p = cy + radius * np.sin(t) else: - # Parametric equation: Center + scale * major_std * v_major * cos(t) + scale * minor_std * v_minor * sin(t) - # Note: We use scale*std which corresponds to half-axis lengths a,b used before + # Parametric equation using eigenvectors and eigenvalues t = np.linspace(0, 2 * np.pi, N) cos_t, sin_t = np.cos(t), np.sin(t) x_p = cx + scale * ( @@ -66,7 +64,9 @@ def create_ellipse_path_from_cov( # --- Plotly Element Generators --- -def create_gt_landmarks_trace(landmarks_gt: np.ndarray) -> Optional[go.Scatter]: +def create_gt_landmarks_trace( + landmarks_gt: Optional[np.ndarray], +) -> Optional[go.Scatter]: """Creates scatter trace for ground truth landmarks.""" if landmarks_gt is None or landmarks_gt.size == 0: return None @@ -79,7 +79,7 @@ def create_gt_landmarks_trace(landmarks_gt: np.ndarray) -> Optional[go.Scatter]: ) -def create_gt_path_trace(poses_gt: List[gtsam.Pose2]) -> Optional[go.Scatter]: +def create_gt_path_trace(poses_gt: Optional[List[gtsam.Pose2]]) -> Optional[go.Scatter]: """Creates line trace for ground truth path.""" if not poses_gt: return None @@ -95,21 +95,36 @@ def create_gt_path_trace(poses_gt: List[gtsam.Pose2]) -> Optional[go.Scatter]: def create_est_path_trace( est_path_x: List[float], est_path_y: List[float] ) -> go.Scatter: - """Creates trace for the estimated path.""" + """Creates trace for the estimated path (all poses up to current).""" return go.Scatter( x=est_path_x, y=est_path_y, mode="lines+markers", - line=dict(color="red", width=2), - marker=dict(size=4, color="red"), + line=dict(color="rgba(255, 0, 0, 0.3)", width=1), # Fainter line for history + marker=dict(size=4, color="red"), # Keep markers prominent name="Path Est", ) +def create_current_pose_trace( + current_pose: Optional[gtsam.Pose2], +) -> Optional[go.Scatter]: + """Creates a single marker trace for the current estimated pose.""" + if current_pose is None: + return None + return go.Scatter( + x=[current_pose.x()], + y=[current_pose.y()], + mode="markers", + marker=dict(color="magenta", size=10, symbol="cross"), + name="Current Pose", + ) + + def create_est_landmarks_trace( est_landmarks_x: List[float], est_landmarks_y: List[float] ) -> Optional[go.Scatter]: - """Creates trace for estimated landmarks.""" + """Creates trace for currently estimated landmarks.""" if not est_landmarks_x: return None return go.Scatter( @@ -122,7 +137,7 @@ def create_est_landmarks_trace( def _create_ellipse_shape_dict( - cx, cy, cov, scale, fillcolor, line_color + cx: float, cy: float, cov: np.ndarray, scale: float, fillcolor: str, line_color: str ) -> Dict[str, Any]: """Helper: Creates dict for a Plotly ellipse shape from covariance.""" path = create_ellipse_path_from_cov(cx, cy, cov, scale) @@ -133,6 +148,7 @@ def _create_ellipse_shape_dict( yref="y", fillcolor=fillcolor, line_color=line_color, + opacity=0.7, # Make ellipses slightly transparent ) @@ -145,8 +161,8 @@ def create_pose_ellipse_shape( cy=pose_mean_xy[1], cov=pose_cov, scale=scale, - fillcolor="rgba(255,0,255,0.2)", - line_color="rgba(255,0,255,0.5)", + fillcolor="rgba(255,0,255,0.2)", # Magenta fill + line_color="rgba(255,0,255,0.5)", # Magenta line ) @@ -159,8 +175,8 @@ def create_landmark_ellipse_shape( cy=lm_mean_xy[1], cov=lm_cov, scale=scale, - fillcolor="rgba(0,0,255,0.1)", - line_color="rgba(0,0,255,0.3)", + fillcolor="rgba(0,0,255,0.1)", # Blue fill + line_color="rgba(0,0,255,0.3)", # Blue line ) @@ -171,18 +187,12 @@ def dot_string_to_base64_svg( if not dot_string: return None try: - # Set DPI for potentially better default sizing, though less critical for SVG - # graph_attr = {'dpi': '150'} # You can experiment with this - source = graphviz.Source(dot_string, engine=engine) # , graph_attr=graph_attr) - # Use pipe() to get bytes directly without saving a file, format="svg" + source = graphviz.Source(dot_string, engine=engine) svg_bytes = source.pipe(format="svg") - # Encode bytes to base64 string, decode result to UTF-8 string encoded_string = base64.b64encode(svg_bytes).decode("utf-8") - # Return data URI for SVG return f"data:image/svg+xml;base64,{encoded_string}" except graphviz.backend.execute.CalledProcessError as e: print(f"Graphviz rendering error ({engine}): {e}") - # Optionally return a placeholder or raise return None except Exception as e: print(f"Unexpected error during Graphviz SVG generation: {e}") @@ -201,65 +211,77 @@ def generate_frame_content( ) -> Tuple[List[go.Scatter], List[Dict[str, Any]], Optional[Dict[str, Any]]]: """Generates dynamic traces, shapes, and layout image for a single frame.""" k = frame_data.step_index + # Use the results specific to this frame for current elements step_results = frame_data.results step_marginals = frame_data.marginals - frame_dynamic_traces: List[go.Scatter] = [] # Renamed for clarity + frame_dynamic_traces: List[go.Scatter] = [] frame_shapes: List[Dict[str, Any]] = [] layout_image: Optional[Dict[str, Any]] = None - # 1. Estimated Path - est_path_x = [ - step_results.atPose2(X(i)).x() - for i in range(k + 1) - if step_results.exists(X(i)) - ] - est_path_y = [ - step_results.atPose2(X(i)).y() - for i in range(k + 1) - if step_results.exists(X(i)) - ] - frame_dynamic_traces.append(create_est_path_trace(est_path_x, est_path_y)) + # 1. Estimated Path (Full History or Partial) + est_path_x = [] + est_path_y = [] + current_pose_est = None - # 2. Estimated Landmarks + # Plot poses currently existing in the step_results (e.g., within lag) + for i in range(k + 1): # Check poses up to current step index + pose_key = X(i) + if step_results.exists(pose_key): + pose = step_results.atPose2(pose_key) + est_path_x.append(pose.x()) + est_path_y.append(pose.y()) + if i == k: + current_pose_est = pose + + path_trace = create_est_path_trace(est_path_x, est_path_y) + if path_trace: + frame_dynamic_traces.append(path_trace) + + # Add a distinct marker for the current pose estimate + current_pose_trace = create_current_pose_trace(current_pose_est) + if current_pose_trace: + frame_dynamic_traces.append(current_pose_trace) + + # 2. Estimated Landmarks (Only those present in step_results) est_landmarks_x, est_landmarks_y, landmark_keys = [], [], [] for j in range(max_landmark_index + 1): lm_key = L(j) + # Check existence in the results for the *current frame* if step_results.exists(lm_key): lm_point = step_results.atPoint2(lm_key) est_landmarks_x.append(lm_point[0]) est_landmarks_y.append(lm_point[1]) - landmark_keys.append(lm_key) + landmark_keys.append(lm_key) # Store keys for covariance lookup + lm_trace = create_est_landmarks_trace(est_landmarks_x, est_landmarks_y) if lm_trace: frame_dynamic_traces.append(lm_trace) - # 3. Covariance Ellipses + # 3. Covariance Ellipses (Only for items in step_results AND step_marginals) if step_marginals: + # Current Pose Ellipse pose_key = X(k) - if step_results.exists(pose_key): - try: - cov = step_marginals.marginalCovariance(pose_key) - mean = step_results.atPose2(pose_key).translation() - frame_shapes.append(create_pose_ellipse_shape(mean, cov, ellipse_scale)) - except RuntimeError as e: # More specific exception type - if verbose: - print(f"Warn: Pose {k} cov err @ step {k}: {e}") - except Exception as e: - if verbose: - print(f"Warn: Pose {k} cov OTHER err @ step {k}: {e}") + # Retrieve cov from marginals specific to this frame + cov = step_marginals.marginalCovariance(pose_key) + # Ensure mean comes from the pose in current results + mean = step_results.atPose2(pose_key).translation() + frame_shapes.append(create_pose_ellipse_shape(mean, cov, ellipse_scale)) + # Landmark Ellipses (Iterate over keys found in step_results) for lm_key in landmark_keys: try: + # Retrieve cov from marginals specific to this frame cov = step_marginals.marginalCovariance(lm_key) + # Ensure mean comes from the landmark in current results mean = step_results.atPoint2(lm_key) frame_shapes.append( create_landmark_ellipse_shape(mean, cov, ellipse_scale) ) - except RuntimeError as e: # More specific exception type + except RuntimeError: # Covariance might not be available if verbose: print( - f"Warn: LM {gtsam.Symbol(lm_key).index()} cov err @ step {k}: {e}" + f"Warn: LM {gtsam.Symbol(lm_key).index()} cov not in marginals @ step {k}" ) except Exception as e: if verbose: @@ -267,9 +289,8 @@ def generate_frame_content( f"Warn: LM {gtsam.Symbol(lm_key).index()} cov OTHER err @ step {k}: {e}" ) - # 4. Graph Image for Layout (MODIFIED TO USE SVG) - # Use the SVG function with the dot string from frame_data - img_src = dot_string_to_base64_svg( # Changed function call + # 4. Graph Image for Layout + img_src = dot_string_to_base64_svg( frame_data.graph_dot_string, engine=graphviz_engine ) if img_src: @@ -284,10 +305,10 @@ def generate_frame_content( xanchor="left", yanchor="top", layer="below", - sizing="contain", # Contain should work well with SVG + sizing="contain", ) - # Return only the things that CHANGE frame-to-frame + # Return dynamic elements for this frame return frame_dynamic_traces, frame_shapes, layout_image @@ -336,7 +357,7 @@ def configure_figure_layout( x=plot_domain[0], xanchor="left", y=0, - yanchor="top", # Position relative to plot area + yanchor="top", buttons=[ dict( label="Play", @@ -371,35 +392,35 @@ def configure_figure_layout( title="Factor Graph SLAM Animation (Graph Left, Results Right)", xaxis=dict( range=[-world_size / 2 - 2, world_size / 2 + 2], - domain=plot_domain, # Confine axis to right pane + domain=plot_domain, constrain="domain", ), yaxis=dict( range=[-world_size / 2 - 2, world_size / 2 + 2], scaleanchor="x", scaleratio=1, - domain=[0, 1], # Full height within its domain + domain=[0, 1], ), width=1000, - height=600, # Adjust size for side-by-side + height=600, hovermode="closest", updatemenus=updatemenus, sliders=sliders, - shapes=initial_shapes, # Initial shapes for the SLAM plot - images=( - [initial_image] if initial_image else [] - ), # Initial image for the left pane - showlegend=False, # Forego legend for space - # Autosize=True might help responsiveness but can interfere with domain + shapes=initial_shapes, # Initial shapes (frame 0) + images=([initial_image] if initial_image else []), # Initial image (frame 0) + showlegend=True, # Keep legend for clarity + legend=dict( + x=plot_domain[0], + y=1, + traceorder="normal", # Position legend + bgcolor="rgba(255,255,255,0.5)", + ), ) # --- Main Animation Orchestrator --- -# --- Main Animation Orchestrator (MODIFIED TO FIX GT DISPLAY) --- - - def create_slam_animation( history: List[SlamFrameData], X: Callable[[int], int], @@ -447,8 +468,7 @@ def create_slam_animation( verbose_cov_errors, ) - # 3. Add initial traces to the figure (GT first, then initial dynamic traces) - # These define the plot state BEFORE the animation starts or when slider is at 0 + # 3. Add initial traces (GT + dynamic frame 0) for trace in gt_traces: fig.add_trace(trace) for trace in initial_dynamic_traces: @@ -457,24 +477,12 @@ def create_slam_animation( # 4. Generate frames for the animation (k=0 to num_steps) frames = [] steps_iterable = range(num_steps + 1) - try: - steps_iterable = tqdm(steps_iterable, desc="Creating Frames") - except NameError: # Handle if tqdm is not available (e.g. standard Python script) - pass - except Exception as e: # Catch other potential tqdm issues - print( - f"Note: Could not initialize tqdm progress bar ({e}). Continuing without it." - ) + steps_iterable = tqdm(steps_iterable, desc="Creating Frames") for k in steps_iterable: frame_data = next((item for item in history if item.step_index == k), None) - if frame_data is None: - print(f"Warning: Missing data for step {k} in history. Skipping frame.") - # Optionally create an empty frame or reuse previous frame's data - # For simplicity, we skip here, which might cause jumps in animation - continue - # Generate dynamic content for this specific frame + # Generate dynamic content specific to this frame frame_dynamic_traces, frame_shapes, layout_image = generate_frame_content( frame_data, X, @@ -485,14 +493,18 @@ def create_slam_animation( verbose_cov_errors, ) - # IMPORTANT: Combine static GT traces with the dynamic traces for THIS frame - # The 'data' in the frame replaces the figure's data for that step. + # Frame definition: includes static GT + dynamic traces for this step + # Layout updates only include shapes and images for this step frames.append( go.Frame( - data=gt_traces + frame_dynamic_traces, # Include GT in every frame + data=gt_traces + + frame_dynamic_traces, # GT must be in each frame's data name=str(k), - layout=go.Layout( # Only update shapes and images in layout per frame - shapes=frame_shapes, images=[layout_image] if layout_image else [] + layout=go.Layout( + shapes=frame_shapes, # Replaces shapes list for this frame + images=( + [layout_image] if layout_image else [] + ), # Replaces image list ), ) ) @@ -501,7 +513,6 @@ def create_slam_animation( fig.update(frames=frames) # 6. Configure overall layout (sliders, buttons, axes, etc.) - # Pass the shapes and image corresponding to the INITIAL state (k=0) configure_figure_layout(fig, num_steps, world_size, initial_shapes, initial_image) print("Plotly animation generated.")