2. Omega and Xi, Constraints.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Omega and Xi\n",
    "\n",
    "To implement Graph SLAM, a matrix and a vector (omega and xi, respectively) are introduced. The matrix is square and labelled with all the robot poses (xi) and all the landmarks (Li). Every time you make an observation, for example, as you move between two poses by some distance `dx` and can relate those two positions, you can represent this as a numerical relationship in these matrices.\n",
    "\n",
    "It's easiest to see how these work in an example. Below you can see a matrix representation of omega and a vector representation of xi.\n",
    "\n",
    "<img src='i/https://github.com/sunsided/slam/blob/master/mages/omega_xi.png' width=20% height=20% />\n",
    "\n",
    "Next, let's look at a simple example that relates 3 poses to one another. \n",
    "* When you start out in the world most of these values are zeros or contain only values from the initial robot position\n",
    "* In this example, you have been given constraints, which relate these poses to one another\n",
    "* Constraints translate into matrix values\n",
    "\n",
    "<img src='i/https://github.com/sunsided/slam/blob/master/mages/omega_xi_constraints.png' width=70% height=70% />\n",
    "\n",
    "If you have ever solved linear systems of equations before, this may look familiar, and if not, let's keep going!\n",
    "\n",
    "### Solving for x\n",
    "\n",
    "To \"solve\" for all these x values, we can use linear algebra; all the values of x are in the vector `mu` which can be calculated as a product of the inverse of omega times xi.\n",
    "\n",
    "<img src='i/https://github.com/sunsided/slam/blob/master/mages/solution.png' width=30% height=30% />\n",
    "\n",
    "---\n",
    "**You can confirm this result for yourself by executing the math in the cell below.**\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-3.]\n",
      " [ 2.]\n",
      " [ 5.]]\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "\n",
    "# x0 = -3       ->   x0      =  3  ->   1  0  0  =  3\n",
    "# x1 = x0 + 5   ->  -x0 + x1 =  5  ->  -1  1  0  =  5\n",
    "#                    x0 - x1 = -5  ->   1 -1  0  = -5\n",
    "# x2 = x1 + 3   ->  -x1 + x2 =  3  ->   0 -1  1  =  3\n",
    "#                    x1 - x2 = -3  ->   0  1 -1  = -3\n",
    "\n",
    "# define omega and xi as in the example\n",
    "omega = np.array([[1,0,0],\n",
    "                  [-1,1,0],\n",
    "                  [0,-1,1]])\n",
    "\n",
    "xi = np.array([[-3],\n",
    "               [5],\n",
    "               [3]])\n",
    "\n",
    "# calculate the inverse of omega\n",
    "omega_inv = np.linalg.inv(np.matrix(omega))\n",
    "\n",
    "# calculate the solution, mu\n",
    "mu = omega_inv*xi\n",
    "\n",
    "# print out the values of mu (x0, x1, x2)\n",
    "print(mu)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Motion Constraints and Landmarks\n",
    "\n",
    "In the last example, the constraint equations, relating one pose to another were given to you. In this next example, let's look at how motion (and similarly, sensor measurements) can be used to create constraints and fill up the constraint matrices, omega and xi. Let's start with empty/zero matrices.\n",
    "\n",
    "<img src='i/https://github.com/sunsided/slam/blob/master/mages/initial_constraints.png' width=35% height=35% />\n",
    "\n",
    "This example also includes relationships between poses and landmarks. Say we move from x0 to x1 with a displacement `dx` of 5. Then we have created a motion constraint that relates x0 to x1, and we can start to fill up these matrices.\n",
    "\n",
    "<img src='i/https://github.com/sunsided/slam/blob/master/mages/motion_constraint.png' width=50% height=50% />\n",
    "\n",
    "In fact, the one constraint equation can be written in two ways. So, the motion constraint that relates x0 and x1 by the motion of 5 has affected the matrix, adding values for *all* elements that correspond to x0 and x1.\n",
    "\n",
    "### 2D case\n",
    "\n",
    "In these examples, we've been showing you change in only one dimension, the x-dimension. In the project, it will be up to you to represent x and y positional values in omega and xi. One solution could be to create an omega and xi that are 2x larger, so that they can hold both x and y values for poses. I might suggest drawing out a rough solution to graph slam as you read the instructions in the next notebook; that always helps me organize my thoughts. Good luck!"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python [conda env:cvnd-slam]",
   "language": "python",
   "name": "conda-env-cvnd-slam-py"
  },
  "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.6.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}