Converted PlanarSLAM
Frank Dellaert
parent
c9498fe0c0
commit
247172dae8
|
|
@ -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": [
|
||||
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/examples/PlanarSLAMExample.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
|
||||
]
|
||||
},
|
||||
{
|
||||
"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
|
||||
}
|
||||
|
|
@ -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()
|
||||
Loading…
Reference in New Issue