Pose2SLAMExample_g2o.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "463ab0e3",
   "metadata": {},
   "source": [
    "<a href=\"https://colab.research.google.com/github/gtbook/gtsam-examples/blob/main/Pose2SLAMExample_g2o.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "4fa834f8",
   "metadata": {},
   "source": [
    "# Pose2 SLAM with g2o Files\n",
    "\n",
    "The example below is a Pose SLAM example that reads from \"g2o\" files:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "a4998929",
   "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": "36947065",
   "metadata": {},
   "outputs": [],
   "source": [
    "import gtsam\n",
    "import matplotlib.pyplot as plt\n",
    "from gtsam.utils import plot"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "b2f11570",
   "metadata": {},
   "source": [
    "Set some parameters:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "dc78fb27",
   "metadata": {},
   "outputs": [],
   "source": [
    "maxIterations = 100"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "48722579",
   "metadata": {},
   "source": [
    "Read the file:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "fe146635",
   "metadata": {},
   "outputs": [],
   "source": [
    "g2oFile = gtsam.findExampleDataFile(\"noisyToyGraph.txt\")\n",
    "graph, initial = gtsam.readG2o(g2oFile, is3D=False)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "81d31867",
   "metadata": {},
   "source": [
    "Add prior on the pose having index (key) = 0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "b944930a",
   "metadata": {},
   "outputs": [],
   "source": [
    "priorModel = gtsam.noiseModel.Diagonal.Variances(\n",
    "    gtsam.Point3(1e-6, 1e-6, 1e-8))\n",
    "graph.add(gtsam.PriorFactorPose2(0, gtsam.Pose2(), priorModel))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d2cf9b91",
   "metadata": {},
   "source": [
    "Create Gauss-Newton optimizer and optimize:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "4bf3feb9",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Optimization complete\n",
      "initial error =  0.3916375099488787\n",
      "final error =  0.06850346649982364\n",
      "Warning:  stopping nonlinear iterations because error increased\n",
      "errorThreshold: 0.0685035 <? 0\n",
      "absoluteDecrease: -2.28853697639e-05 <? 1e-05\n",
      "relativeDecrease: -0.000334187727181 <? 1e-05\n",
      "iterations: 3 >? 100\n"
     ]
    }
   ],
   "source": [
    "params = gtsam.GaussNewtonParams()\n",
    "params.setVerbosity(\"Termination\")\n",
    "params.setMaxIterations(maxIterations)\n",
    "# parameters.setRelativeErrorTol(1e-5)\n",
    "# Create the optimizer ...\n",
    "optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)\n",
    "# ... and optimize\n",
    "result = optimizer.optimize()\n",
    "\n",
    "print(\"Optimization complete\")\n",
    "print(\"initial error = \", graph.error(initial))\n",
    "print(\"final error = \", graph.error(result))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "8508d7e1",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "\n",
      "Factor Graph:\n",
      "NonlinearFactorGraph: size: 6\n",
      "\n",
      "Factor 0: BetweenFactor(0,1)\n",
      "  measured:  (0.774115, 1.183389, 1.576173)\n",
      "  noise model: unit (3) \n",
      "\n",
      "Factor 1: BetweenFactor(1,2)\n",
      "  measured:  (0.869231, 1.031877, 1.579418)\n",
      "  noise model: unit (3) \n",
      "\n",
      "Factor 2: BetweenFactor(2,3)\n",
      "  measured:  (1.35784, 1.034262, 1.56646)\n",
      "  noise model: unit (3) \n",
      "\n",
      "Factor 3: BetweenFactor(2,0)\n",
      "  measured:  (0.303492, 1.865011, -3.113898)\n",
      "  noise model: unit (3) \n",
      "\n",
      "Factor 4: BetweenFactor(0,3)\n",
      "  measured:  (-0.928526, 0.993695, -1.563542)\n",
      "  noise model: unit (3) \n",
      "\n",
      "Factor 5: PriorFactor on 0\n",
      "  prior mean:  (0, 0, 0)\n",
      "  noise model: diagonal sigmas [0.001; 0.001; 0.0001];\n",
      "\n",
      "\n",
      "\n",
      "Initial Estimate:\n",
      "Values with 4 values:\n",
      "Value 0: (gtsam::Pose2)\n",
      "(0, 0, 0)\n",
      "\n",
      "Value 1: (gtsam::Pose2)\n",
      "(0.774115, 1.183389, 1.576173)\n",
      "\n",
      "Value 2: (gtsam::Pose2)\n",
      "(-0.26242, 2.047059, -3.127594)\n",
      "\n",
      "Value 3: (gtsam::Pose2)\n",
      "(-1.605649, 0.993891, -1.561134)\n",
      "\n",
      "\n",
      "Final Result:\n",
      "Values with 4 values:\n",
      "Value 0: (gtsam::Pose2)\n",
      "(-7.17032659e-23, 8.60267581e-24, 7.6514202e-25)\n",
      "\n",
      "Value 1: (gtsam::Pose2)\n",
      "(0.95617894, 1.14319823, 1.54150027)\n",
      "\n",
      "Value 2: (gtsam::Pose2)\n",
      "(0.126611466, 1.98179453, -3.08397402)\n",
      "\n",
      "Value 3: (gtsam::Pose2)\n",
      "(-1.05038303, 0.935131621, -1.54052801)\n",
      "\n",
      "\n"
     ]
    }
   ],
   "source": [
    "print(\"\\nFactor Graph:\\n{}\".format(graph))\n",
    "print(\"\\nInitial Estimate:\\n{}\".format(initial))\n",
    "print(\"Final Result:\\n{}\".format(result))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "d0af71d8",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "resultPoses = gtsam.utilities.extractPose2(result)\n",
    "for i in range(resultPoses.shape[0]):\n",
    "    plot.plot_pose2(1, gtsam.Pose2(resultPoses[i, :]))\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "287431d9",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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
}