localization_exercises/4_7_SLAM/1_1. Construct Omega and Xi, exercise.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",
    "Below you can see a matrix representation of omega and a vector representation of xi.\n",
    "\n",
    "<img src='images/omega_xi.png' width=20% height=20% />\n",
    "\n",
    "\n",
    "### Solving for x, L\n",
    "\n",
    "To \"solve\" for all these poses and landmark positions, we can use linear algebra; all the positional values are in the vector `mu` which can be calculated as a product of the inverse of omega times xi.\n",
    "\n",
    "---\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Quiz: Construct constraints for 3 motions and return `mu`\n",
    "\n",
    "In the following example, you will complete the function call `mu_from_positions(-3, 5, 3)`, which takes in 3 robot poses/moves:\n",
    "* initial pose: -3\n",
    "* moves by 5\n",
    "* moves by 3\n",
    "\n",
    "In this function, you should construct the constraint matrices `omega` and `xi` and calculate `mu`. The final call should result in a `mu` of:\n",
    "```\n",
    "[[-3.0],\n",
    " [2.0],\n",
    " [5.0]]\n",
    " ```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Constraint Updates\n",
    "\n",
    "We will not consider landmark sensor measurements in this example, only robot poses.\n",
    "\n",
    "#### Motion\n",
    "When your robot moves by some amount `dx` update the constraint matrices as follows:\n",
    "* Add `[[1, -1], [-1, 1]]` to omega at the indices for the intersection of `xt` and `xt+1`\n",
    "* Add `-dx` and `dx` to xi at the rows for `xt` and `xt+1`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "def mu_from_positions(initial_pos, move1, move2):\n",
    "    \n",
    "    ## construct constraint matrices\n",
    "    ## and add each position/motion constraint to them\n",
    "    \n",
    "    # initialize constraint matrices wit zeros\n",
    "    omega = np.zeros((3,3))\n",
    "    xi = np.zeros((3,1))\n",
    "    \n",
    "    # add initial pose constraint\n",
    "    omega[0][0] = 1\n",
    "    xi[0] = initial_pos\n",
    "    \n",
    "    # first motion \n",
    "    omega += [[1 ,-1, 0],\n",
    "              [-1, 1, 0],\n",
    "              [0 , 0, 0]]\n",
    "    \n",
    "    xi += [[-move1],\n",
    "           [move1],\n",
    "           [0]]\n",
    "    \n",
    "    # second motion \n",
    "    omega += [[0 , 0, 0],\n",
    "              [0 , 1,-1],\n",
    "              [0 ,-1, 1]]\n",
    "    \n",
    "    xi += [[0],\n",
    "           [-move2],\n",
    "           [move2]]\n",
    "    \n",
    "    # display final omega and xi\n",
    "    print('Omega: \\n', omega)\n",
    "    print('\\n')\n",
    "    print('Xi: \\n', xi)\n",
    "    print('\\n')\n",
    "    \n",
    "    ## calculate mu as the inverse of omega * xi\n",
    "    ## recommended that you use: np.linalg.inv(np.matrix(omega)) to calculate the inverse\n",
    "    omega_inv = np.linalg.inv(np.matrix(omega))\n",
    "    mu = omega_inv*xi\n",
    "    return mu\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Omega: \n",
      " [[ 2. -1.  0.]\n",
      " [-1.  2. -1.]\n",
      " [ 0. -1.  1.]]\n",
      "\n",
      "\n",
      "Xi: \n",
      " [[-8.]\n",
      " [ 2.]\n",
      " [ 3.]]\n",
      "\n",
      "\n",
      "Mu: \n",
      " [[-3.]\n",
      " [ 2.]\n",
      " [ 5.]]\n"
     ]
    }
   ],
   "source": [
    "# call function and print out `mu`\n",
    "mu = mu_from_positions(-3, 5, 3)\n",
    "print('Mu: \\n', mu)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.7.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}