{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "abdd4aba",
   "metadata": {},
   "source": [
    "<a href=\"https://colab.research.google.com/github/gtbook/gtsam-examples/blob/main/PlanarSLAMExample.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "52823359",
   "metadata": {},
   "source": [
    "# Planar SLAM\n",
    "\n",
    "\"True\" Simultaneous Localization and Mapping optimizes for both the unknown robot trajectory and unknown *landmarks*. The simple example below sketches out how this would work with GTSAM, in the 2D (`Pose2`) case."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "9896f5de",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Note: you may need to restart the kernel to use updated packages.\n"
     ]
    }
   ],
   "source": [
    "%pip -q install gtbook # also installs latest gtsam pre-release"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "0baa45d2",
   "metadata": {},
   "outputs": [],
   "source": [
    "import gtsam\n",
    "import numpy as np\n",
    "from gtsam.symbol_shorthand import L, X"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "63f5d46f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create noise models\n",
    "PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))\n",
    "ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n",
    "MEASUREMENT_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.2]))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "ed06aebf",
   "metadata": {},
   "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": [
    "# Create an empty nonlinear factor graph\n",
    "graph = gtsam.NonlinearFactorGraph()\n",
    "\n",
    "# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model\n",
    "graph.add(\n",
    "    gtsam.PriorFactorPose2(X(1), gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))\n",
    "\n",
    "# Add odometry factors between X1,X2 and X2,X3, respectively\n",
    "graph.add(\n",
    "    gtsam.BetweenFactorPose2(X(1), X(2), gtsam.Pose2(2.0, 0.0, 0.0),\n",
    "                             ODOMETRY_NOISE))\n",
    "graph.add(\n",
    "    gtsam.BetweenFactorPose2(X(2), X(3), gtsam.Pose2(2.0, 0.0, 0.0),\n",
    "                             ODOMETRY_NOISE))\n",
    "\n",
    "# Add Range-Bearing measurements to two different landmarks L1 and L2\n",
    "graph.add(\n",
    "    gtsam.BearingRangeFactor2D(X(1), L(1), gtsam.Rot2.fromDegrees(45),\n",
    "                               np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE))\n",
    "graph.add(\n",
    "    gtsam.BearingRangeFactor2D(X(2), L(1), gtsam.Rot2.fromDegrees(90), 2.0,\n",
    "                               MEASUREMENT_NOISE))\n",
    "graph.add(\n",
    "    gtsam.BearingRangeFactor2D(X(3), L(2), gtsam.Rot2.fromDegrees(90), 2.0,\n",
    "                               MEASUREMENT_NOISE))\n",
    "\n",
    "# Print graph\n",
    "print(\"Factor Graph:\\n{}\".format(graph))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "e7efe6dc",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Initial Estimate:\n",
      "Values with 5 values:\n",
      "Value l1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)\n",
      "[\n",
      "\t1.8;\n",
      "\t2.1\n",
      "]\n",
      "\n",
      "Value l2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)\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",
    "initial_estimate = gtsam.Values()\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",
    "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\n",
    "print(\"Initial Estimate:\\n{}\".format(initial_estimate))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1c9c9b1b",
   "metadata": {},
   "source": [
    "Optimize using Levenberg-Marquardt optimization.\n",
    "\n",
    "The optimizer accepts an optional set of configuration parameters, controlling\n",
    "things like convergence criteria, the type of linear system solver\n",
    "to use, and the amount of information displayed during optimization.\n",
    "Here we will use the default set of parameters.  See the\n",
    "documentation for the full set of parameters.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "afe0d8b8",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Final Result:\n",
      "Values with 5 values:\n",
      "Value l1: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)\n",
      "[\n",
      "\t2;\n",
      "\t2\n",
      "]\n",
      "\n",
      "Value l2: (Eigen::Matrix<double, -1, 1, 0, -1, 1>)\n",
      "[\n",
      "\t4;\n",
      "\t2\n",
      "]\n",
      "\n",
      "Value x1: (gtsam::Pose2)\n",
      "(-5.72151495e-16, -2.62210315e-16, -8.93526163e-17)\n",
      "\n",
      "Value x2: (gtsam::Pose2)\n",
      "(2, -5.54361422e-15, -7.37201189e-16)\n",
      "\n",
      "Value x3: (gtsam::Pose2)\n",
      "(4, -1.04971108e-14, -6.96394173e-16)\n",
      "\n",
      "\n"
     ]
    }
   ],
   "source": [
    "params = gtsam.LevenbergMarquardtParams()\n",
    "optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate,\n",
    "                                              params)\n",
    "result = optimizer.optimize()\n",
    "print(\"\\nFinal Result:\\n{}\".format(result))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c5fac411",
   "metadata": {},
   "source": [
    "Calculate and print marginal covariances for all variables"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "fb52988e",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "X1 covariance:\n",
      "[[ 9.00000000e-02 -4.90734592e-32 -1.91846539e-17]\n",
      " [-4.90734592e-32  9.00000000e-02  2.55795385e-17]\n",
      " [-1.91846539e-17  2.55795385e-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": [
    "marginals = gtsam.Marginals(graph, result)\n",
    "for (key, s) in [(X(1), \"X1\"), (X(2), \"X2\"), (X(3), \"X3\"), (L(1), \"L1\"),\n",
    "                 (L(2), \"L2\")]:\n",
    "    print(\"{} covariance:\\n{}\\n\".format(s,\n",
    "                                        marginals.marginalCovariance(key)))"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "341996cd3f3db7b5e0d1eaea072c5502d80452314e72e6b77c40445f6e9ba101"
  },
  "kernelspec": {
   "display_name": "Python 3.8.12 ('nbdev')",
   "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.8.12"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}