{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import copy\n",
    "import math\n",
    "import itertools\n",
    "import numpy as np \n",
    "import cvxpy as cp\n",
    "import matplotlib.pyplot as plt\n",
    "from graph_based_slam import calc_rotational_matrix, calc_jacobian, cal_observation_sigma, \\\n",
    "                             calc_input, observation, motion_model, Edge, pi_2_pi\n",
    "\n",
    "%matplotlib inline\n",
    "np.set_printoptions(precision=3, suppress=True)\n",
    "np.random.seed(0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Introduction\n",
    "\n",
    "In contrast to the probabilistic approaches for solving SLAM, such as EKF, UKF, particle filters, and so on, the graph technique formulates the SLAM as an optimization problem.  It is mostly used to solve the full SLAM problem in an offline fashion, i.e. optimize all the poses of the robot after the path has been traversed. However, some variants are availble that uses graph-based approaches to perform online estimation or to solve for a subset of the poses.\n",
    "\n",
    "GraphSLAM uses the motion information as well as the observations of the environment to create least square problem that can be solved using standard optimization techniques.\n",
    "\n",
    "### Minimal Example\n",
    "The following example illustrates the main idea behind graphSLAM. \n",
    "A simple case of a 1D robot is considered that can only move in 1 direction. The robot is commanded to move forward with a control input $u_t=1$, however, the motion is not perfect and the measured odometry will deviate from the true path. At each timestep the robot can observe its environment, for this simple case as well, there is only a single landmark at coordinates $x=3$. The measured observations are the range between the robot and landmark. These measurements are also subjected to noise. No bearing information is required since this is a 1D problem.\n",
    "\n",
    "To solve this, graphSLAM creates what is called as the system information matrix $\\Omega$ also referred to as $H$ and the information vector $\\xi$ also known as $b$. The entries are created based on the information of the motion and the observation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 11456 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The determinant of H:  0.0\n",
      "The determinant of H after anchoring constraint:  18.75\n",
      "Running graphSLAM ...\n",
      "Odometry values after optimzation: \n",
      " [[0.   ]\n",
      " [0.947]\n",
      " [1.939]]\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 11524 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "R = 0.2\n",
    "Q = 0.2\n",
    "N = 3\n",
    "graphics_radius = 0.1\n",
    "\n",
    "odom = np.empty((N,1))\n",
    "obs = np.empty((N,1))\n",
    "x_true = np.empty((N,1))\n",
    "\n",
    "landmark = 3\n",
    "# Simulated readings of odometry and observations\n",
    "x_true[0], odom[0], obs[0] = 0.0, 0.0, 2.9\n",
    "x_true[1], odom[1], obs[1] = 1.0, 1.5, 2.0\n",
    "x_true[2], odom[2], obs[2] = 2.0, 2.4, 1.0\n",
    "\n",
    "hxDR = copy.deepcopy(odom)\n",
    "# Visualization\n",
    "plt.plot(landmark,0, '*k', markersize=30)\n",
    "for i in range(N):\n",
    "    plt.plot(odom[i], 0, '.', markersize=50, alpha=0.8, color='steelblue')\n",
    "    plt.plot([odom[i], odom[i] + graphics_radius],\n",
    "             [0,0], 'r')\n",
    "    plt.text(odom[i], 0.02, \"X_{}\".format(i), fontsize=12)\n",
    "    plt.plot(obs[i]+odom[i],0,'.', markersize=25, color='brown')\n",
    "    plt.plot(x_true[i],0,'.g', markersize=20)\n",
    "plt.grid()    \n",
    "plt.show()\n",
    "\n",
    "\n",
    "# Defined as a function to facilitate iteration\n",
    "def get_H_b(odom, obs):\n",
    "    \"\"\"\n",
    "    Create the information matrix and information vector. This implementation is \n",
    "    based on the concept of virtual measurement i.e. the observations of the\n",
    "    landmarks are converted into constraints (edges) between the nodes that\n",
    "    have observed this landmark.\n",
    "    \"\"\"\n",
    "    measure_constraints = {}\n",
    "    omegas = {}\n",
    "    zids = list(itertools.combinations(range(N),2))\n",
    "    H = np.zeros((N,N))\n",
    "    b = np.zeros((N,1))\n",
    "    for (t1, t2) in zids:\n",
    "        x1 = odom[t1]\n",
    "        x2 = odom[t2]\n",
    "        z1 = obs[t1]\n",
    "        z2 = obs[t2]\n",
    "        \n",
    "        # Adding virtual measurement constraint\n",
    "        measure_constraints[(t1,t2)] = (x2-x1-z1+z2)\n",
    "        omegas[(t1,t2)] = (1 / (2*Q))\n",
    "        \n",
    "        # populate system's information matrix and vector\n",
    "        H[t1,t1] += omegas[(t1,t2)]\n",
    "        H[t2,t2] += omegas[(t1,t2)]\n",
    "        H[t2,t1] -= omegas[(t1,t2)]\n",
    "        H[t1,t2] -= omegas[(t1,t2)]\n",
    "\n",
    "        b[t1] += measure_constraints[(t1,t2)]\n",
    "        b[t2] -= measure_constraints[(t1,t2)]\n",
    "\n",
    "    return H, b\n",
    "\n",
    "\n",
    "H, b = get_H_b(odom, obs)\n",
    "print(\"The determinant of H: \", np.linalg.det(H))\n",
    "H[0,0] += 1 # np.inf ?\n",
    "print(\"The determinant of H after anchoring constraint: \", np.linalg.det(H))\n",
    "\n",
    "for i in range(5):\n",
    "    H, b = get_H_b(odom, obs)\n",
    "    H[(0,0)] += 1\n",
    "    \n",
    "    # Recover the posterior over the path\n",
    "    dx = np.linalg.inv(H) @ b\n",
    "    odom += dx\n",
    "    # repeat till convergence\n",
    "print(\"Running graphSLAM ...\")    \n",
    "print(\"Odometry values after optimzation: \\n\", odom)\n",
    "\n",
    "plt.figure()\n",
    "plt.plot(x_true, np.zeros(x_true.shape), '.', markersize=20, label='Ground truth')\n",
    "plt.plot(odom, np.zeros(x_true.shape), '.', markersize=20, label='Estimation')\n",
    "plt.plot(hxDR, np.zeros(x_true.shape), '.', markersize=20, label='Odom')\n",
    "plt.legend()\n",
    "plt.grid()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In particular, the tasks are split into 2 parts, graph construction, and graph optimization. \n",
    "### Graph Construction\n",
    "\n",
    "Firstly the nodes are defined $\\boldsymbol{x} = \\boldsymbol{x}_{1:n}$  such that each node is the pose of the robot at time $t_i$\n",
    "Secondly, the edges i.e. the constraints, are constructed according to the following conditions:\n",
    "\n",
    "* robot moves from $\\boldsymbol{x}_i$ to $\\boldsymbol{x}_j$. This edge corresponds to the odometry measurement. Relative motion constraints (Not included in the previous minimal example).\n",
    "* Measurement constraints, this can be done in two ways:\n",
    "    * The information matrix is set in such a way that it includes the landmarks in the map as well. Then the constraints can be entered in a straightforward fashion between a node $\\boldsymbol{x}_i$ and some landmark $m_k$\n",
    "    * Through creating a virtual measurement among all the node that have observed the same landmark. More concretely, robot observes the same landmark from $\\boldsymbol{x}_i$ and $\\boldsymbol{x}_j$. Relative measurement constraint. The \"virtual measurement\" $\\boldsymbol{z}_{ij}$, which is the estimated pose of $\\boldsymbol{x}_j$ as seen from the node $\\boldsymbol{x}_i$. The virtual measurement can then be entered in the information matrix and vector in a similar fashion to the motion constraints.\n",
    "\n",
    "An edge is fully characterized by the values of the error (entry to information vector) and the local information matrix (entry to the system's information vector). The larger the local information matrix (lower $Q$ or $R$) the values that this edge will contribute with.\n",
    "\n",
    "Important Notes:\n",
    "\n",
    "* The addition to the information matrix and vector are added to the earlier values.\n",
    "* In the case of 2D robot, the constraints will be non-linear, therefore, a Jacobian of the error w.r.t the states is needed when updated $H$ and $b$.\n",
    "* The anchoring constraint is needed in order to avoid having a singular information matri.\n",
    "\n",
    "\n",
    "### Graph Optimization\n",
    "\n",
    "The result from this formulation yields an overdetermined system of equations.\n",
    "The goal after constructing the graph is to find: $x^* = \\underset{x}{\\mathrm{argmin}} ~ \\underset{ij} \\Sigma ~ f(e_{ij}) $, where $f$ is some error function that depends on the edges between to related nodes $i$ and $j$. The derivation in the references arrive at the solution for $x^* = H^{-1}b$ \n",
    "\n",
    "\n",
    "### Planar Example:\n",
    "\n",
    "Now we will go through an example with a more realistic case of a 2D robot with 3DoF, namely, $[x, y, \\theta]^T$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 13428 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "#  Simulation parameter\n",
    "Qsim = np.diag([0.01, np.deg2rad(0.010)])**2 # error added to range and bearing\n",
    "Rsim = np.diag([0.1, np.deg2rad(1.0)])**2 # error added to [v, w]\n",
    "\n",
    "DT = 2.0  # time tick [s]\n",
    "SIM_TIME = 100.0  # simulation time [s]\n",
    "MAX_RANGE = 30.0  # maximum observation range\n",
    "STATE_SIZE = 3  # State size [x,y,yaw]\n",
    "\n",
    "# TODO: Why not use Qsim ? \n",
    "# Covariance parameter of Graph Based SLAM\n",
    "C_SIGMA1 = 0.1\n",
    "C_SIGMA2 = 0.1\n",
    "C_SIGMA3 = np.deg2rad(1.0)\n",
    "\n",
    "MAX_ITR = 20  # Maximum iteration during optimization\n",
    "timesteps = 1\n",
    "\n",
    "# consider only 2 landmarks for simplicity\n",
    "# RFID positions [x, y, yaw]\n",
    "RFID = np.array([[10.0, -2.0, 0.0],\n",
    "#                  [15.0, 10.0, 0.0],\n",
    "#                  [3.0, 15.0, 0.0],\n",
    "#                  [-5.0, 20.0, 0.0],\n",
    "#                  [-5.0, 5.0, 0.0]\n",
    "                 ])\n",
    "\n",
    "# State Vector [x y yaw v]'\n",
    "xTrue = np.zeros((STATE_SIZE, 1))\n",
    "xDR = np.zeros((STATE_SIZE, 1))  # Dead reckoning\n",
    "xTrue[2] = np.deg2rad(45)\n",
    "xDR[2] = np.deg2rad(45)\n",
    "# history initial values\n",
    "hxTrue = xTrue\n",
    "hxDR = xTrue\n",
    "_, z, _, _ = observation(xTrue, xDR, np.array([[0,0]]).T, RFID)\n",
    "hz = [z]\n",
    "\n",
    "for i in range(timesteps):\n",
    "    u = calc_input()\n",
    "    xTrue, z, xDR, ud = observation(xTrue, xDR, u, RFID)\n",
    "    hxDR = np.hstack((hxDR, xDR))\n",
    "    hxTrue = np.hstack((hxTrue, xTrue))\n",
    "    hz.append(z)\n",
    "\n",
    "# visualize\n",
    "graphics_radius = 0.3\n",
    "plt.plot(RFID[:, 0], RFID[:, 1], \"*k\", markersize=20)\n",
    "plt.plot(hxDR[0, :], hxDR[1, :], '.', markersize=50, alpha=0.8, label='Odom')\n",
    "plt.plot(hxTrue[0, :], hxTrue[1, :], '.', markersize=20, alpha=0.6, label='X_true')\n",
    "\n",
    "for i in range(hxDR.shape[1]):\n",
    "    x = hxDR[0, i]\n",
    "    y = hxDR[1, i]\n",
    "    yaw = hxDR[2, i]\n",
    "    plt.plot([x, x + graphics_radius * np.cos(yaw)],\n",
    "             [y, y + graphics_radius * np.sin(yaw)], 'r')\n",
    "    d = hz[i][:, 0]\n",
    "    angle = hz[i][:, 1]\n",
    "    plt.plot([x + d * np.cos(angle + yaw)], [y + d * np.sin(angle + yaw)], '.',\n",
    "             markersize=20, alpha=0.7)\n",
    "    plt.legend()\n",
    "plt.grid()    \n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Copy the data to have a consistent naming with the .py file\n",
    "zlist = copy.deepcopy(hz)\n",
    "x_opt = copy.deepcopy(hxDR)\n",
    "xlist = copy.deepcopy(hxDR)\n",
    "number_of_nodes = x_opt.shape[1]\n",
    "n = number_of_nodes * STATE_SIZE"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "After the data has been saved, the graph will be constructed by looking at each pair for nodes. The virtual measurement is only created if two nodes have observed the same landmark at different points in time. The next cells are a walk through for a single iteration of graph construction -> optimization -> estimate update. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Node combinations: \n",
      " [(0, 1)]\n",
      "Node 0 observed landmark with ID 0.0\n",
      "Node 1 observed landmark with ID 0.0\n"
     ]
    }
   ],
   "source": [
    "# get all the possible combination of the different node\n",
    "zids = list(itertools.combinations(range(len(zlist)), 2))\n",
    "print(\"Node combinations: \\n\", zids)\n",
    "\n",
    "for i in range(xlist.shape[1]):\n",
    "    print(\"Node {} observed landmark with ID {}\".format(i, zlist[i][0, 3]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the following code snippet the error based on the virtual measurement between node 0 and 1 will be created.\n",
    "The equations for the error is as follows: \n",
    "$e_{ij}^x = x_j + d_j cos(\\psi_j + \\theta_j) - x_i - d_i cos(\\psi_i + \\theta_i) $\n",
    "\n",
    "$e_{ij}^y = y_j + d_j sin(\\psi_j + \\theta_j) - y_i - d_i sin(\\psi_i + \\theta_i) $\n",
    "\n",
    "$e_{ij}^x = \\psi_j + \\theta_j - \\psi_i - \\theta_i $\n",
    "\n",
    "Where $[x_i, y_i, \\psi_i]$ is the pose for node $i$ and similarly for node $j$, $d$ is the measured distance at nodes $i$ and $j$, and $\\theta$ is the measured bearing to the landmark. The difference is visualized with the figure in the next cell.\n",
    "\n",
    "In case of perfect motion and perfect measurement the error shall be zero since $ x_j + d_j cos(\\psi_j + \\theta_j)$ should equal $x_i + d_i cos(\\psi_i + \\theta_i)$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "0.0 1.427649841628278 -2.0126109674819155 -3.524048014922737\n",
      "For nodes (0, 1)\n",
      "Added edge with errors: \n",
      " [[-0.02 ]\n",
      " [-0.084]\n",
      " [ 0.   ]]\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 12772 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Initialize edges list\n",
    "edges = []\n",
    "\n",
    "# Go through all the different combinations\n",
    "for (t1, t2) in zids:\n",
    "    x1, y1, yaw1 = xlist[0, t1], xlist[1, t1], xlist[2, t1]\n",
    "    x2, y2, yaw2 = xlist[0, t2], xlist[1, t2], xlist[2, t2]\n",
    "    \n",
    "    # All nodes have valid observation with ID=0, therefore, no data association condition\n",
    "    iz1 = 0\n",
    "    iz2 = 0\n",
    "    \n",
    "    d1 = zlist[t1][iz1, 0]\n",
    "    angle1, phi1 = zlist[t1][iz1, 1], zlist[t1][iz1, 2]\n",
    "    d2 = zlist[t2][iz2, 0]\n",
    "    angle2, phi2 = zlist[t2][iz2, 1], zlist[t2][iz2, 2]\n",
    "    \n",
    "    # find angle between observation and horizontal \n",
    "    tangle1 = pi_2_pi(yaw1 + angle1)\n",
    "    tangle2 = pi_2_pi(yaw2 + angle2)\n",
    "    \n",
    "    # project the observations \n",
    "    tmp1 = d1 * math.cos(tangle1)\n",
    "    tmp2 = d2 * math.cos(tangle2)\n",
    "    tmp3 = d1 * math.sin(tangle1)\n",
    "    tmp4 = d2 * math.sin(tangle2)\n",
    "    \n",
    "    edge = Edge()\n",
    "    print(y1,y2, tmp3, tmp4)\n",
    "    # calculate the error of the virtual measurement\n",
    "    # node 1 as seen from node 2 throught the observations 1,2\n",
    "    edge.e[0, 0] = x2 - x1 - tmp1 + tmp2\n",
    "    edge.e[1, 0] = y2 - y1 - tmp3 + tmp4\n",
    "    edge.e[2, 0] = pi_2_pi(yaw2 - yaw1 - tangle1 + tangle2)\n",
    "\n",
    "    edge.d1, edge.d2 = d1, d2\n",
    "    edge.yaw1, edge.yaw2 = yaw1, yaw2\n",
    "    edge.angle1, edge.angle2 = angle1, angle2\n",
    "    edge.id1, edge.id2 = t1, t2\n",
    "    \n",
    "    edges.append(edge)\n",
    "    \n",
    "    print(\"For nodes\",(t1,t2))\n",
    "    print(\"Added edge with errors: \\n\", edge.e)\n",
    "    \n",
    "    # Visualize measurement projections\n",
    "    plt.plot(RFID[0, 0], RFID[0, 1], \"*k\", markersize=20)\n",
    "    plt.plot([x1,x2],[y1,y2], '.', markersize=50, alpha=0.8)\n",
    "    plt.plot([x1, x1 + graphics_radius * np.cos(yaw1)],\n",
    "             [y1, y1 + graphics_radius * np.sin(yaw1)], 'r')\n",
    "    plt.plot([x2, x2 + graphics_radius * np.cos(yaw2)],\n",
    "             [y2, y2 + graphics_radius * np.sin(yaw2)], 'r')\n",
    "    \n",
    "    plt.plot([x1,x1+tmp1], [y1,y1], label=\"obs 1 x\")\n",
    "    plt.plot([x2,x2+tmp2], [y2,y2], label=\"obs 2 x\")\n",
    "    plt.plot([x1,x1], [y1,y1+tmp3], label=\"obs 1 y\")\n",
    "    plt.plot([x2,x2], [y2,y2+tmp4], label=\"obs 2 y\")\n",
    "    plt.plot(x1+tmp1, y1+tmp3, 'o')\n",
    "    plt.plot(x2+tmp2, y2+tmp4, 'o')\n",
    "plt.legend()\n",
    "plt.grid()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Since the constraints equations derived before are non-linear, linearization is needed before we can insert them into the information matrix and information vector. Two jacobians \n",
    "\n",
    "$A = \\frac{\\partial e_{ij}}{\\partial \\boldsymbol{x}_i}$ as $\\boldsymbol{x}_i$ holds the three variabls x, y, and theta.\n",
    "Similarly, \n",
    "$B = \\frac{\\partial e_{ij}}{\\partial \\boldsymbol{x}_j}$ "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The determinant of H:  0.0\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 4776 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 2 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The determinant of H after origin constraint:  716.1972439134918\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 4776 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 2 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# Initialize the system information matrix and information vector\n",
    "H = np.zeros((n, n))\n",
    "b = np.zeros((n, 1))\n",
    "x_opt = copy.deepcopy(hxDR)\n",
    "\n",
    "for edge in edges:\n",
    "    id1 = edge.id1 * STATE_SIZE\n",
    "    id2 = edge.id2 * STATE_SIZE\n",
    "    \n",
    "    t1 = edge.yaw1 + edge.angle1\n",
    "    A = np.array([[-1.0, 0, edge.d1 * math.sin(t1)],\n",
    "                  [0, -1.0, -edge.d1 * math.cos(t1)],\n",
    "                  [0, 0, -1.0]])\n",
    "\n",
    "    t2 = edge.yaw2 + edge.angle2\n",
    "    B = np.array([[1.0, 0, -edge.d2 * math.sin(t2)],\n",
    "                  [0, 1.0, edge.d2 * math.cos(t2)],\n",
    "                  [0, 0, 1.0]])\n",
    "    \n",
    "    # TODO: use Qsim instead of sigma\n",
    "    sigma = np.diag([C_SIGMA1, C_SIGMA2, C_SIGMA3])\n",
    "    Rt1 = calc_rotational_matrix(tangle1)\n",
    "    Rt2 = calc_rotational_matrix(tangle2)\n",
    "    edge.omega = np.linalg.inv(Rt1 @ sigma @ Rt1.T + Rt2 @ sigma @ Rt2.T)\n",
    "\n",
    "    # Fill in entries in H and b\n",
    "    H[id1:id1 + STATE_SIZE, id1:id1 + STATE_SIZE] += A.T @ edge.omega @ A\n",
    "    H[id1:id1 + STATE_SIZE, id2:id2 + STATE_SIZE] += A.T @ edge.omega @ B\n",
    "    H[id2:id2 + STATE_SIZE, id1:id1 + STATE_SIZE] += B.T @ edge.omega @ A\n",
    "    H[id2:id2 + STATE_SIZE, id2:id2 + STATE_SIZE] += B.T @ edge.omega @ B\n",
    "\n",
    "    b[id1:id1 + STATE_SIZE] += (A.T @ edge.omega @ edge.e)\n",
    "    b[id2:id2 + STATE_SIZE] += (B.T @ edge.omega @ edge.e)\n",
    "   \n",
    "\n",
    "print(\"The determinant of H: \", np.linalg.det(H))\n",
    "plt.figure()\n",
    "plt.subplot(1,2,1)\n",
    "plt.imshow(H, extent=[0, n, 0, n])\n",
    "plt.subplot(1,2,2)\n",
    "plt.imshow(b, extent=[0, 1, 0, n])\n",
    "plt.show()    \n",
    "\n",
    "# Fix the origin, multiply by large number gives same results but better visualization\n",
    "H[0:STATE_SIZE, 0:STATE_SIZE] += np.identity(STATE_SIZE)\n",
    "print(\"The determinant of H after origin constraint: \", np.linalg.det(H))    \n",
    "plt.figure()\n",
    "plt.subplot(1,2,1)\n",
    "plt.imshow(H, extent=[0, n, 0, n])\n",
    "plt.subplot(1,2,2)\n",
    "plt.imshow(b, extent=[0, 1, 0, n])\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "dx: \n",
      " [[ 0.   ]\n",
      " [-0.   ]\n",
      " [ 0.   ]\n",
      " [ 0.02 ]\n",
      " [ 0.084]\n",
      " [-0.   ]]\n",
      "ground truth: \n",
      "  [[0.    1.414]\n",
      " [0.    1.414]\n",
      " [0.785 0.985]]\n",
      "Odom: \n",
      " [[0.    1.428]\n",
      " [0.    1.428]\n",
      " [0.785 0.976]]\n",
      "SLAM: \n",
      " [[ 0.     1.448]\n",
      " [-0.     1.512]\n",
      " [ 0.785  0.976]]\n",
      "\n",
      "graphSLAM localization error:  0.0107290727510571\n",
      "Odom localization error:  0.0004460978857535104\n"
     ]
    }
   ],
   "source": [
    "# Find the solution (first iteration)\n",
    "dx = - np.linalg.inv(H) @ b\n",
    "for i in range(number_of_nodes):\n",
    "    x_opt[0:3, i] += dx[i * 3:i * 3 + 3, 0]\n",
    "print(\"dx: \\n\",dx)\n",
    "print(\"ground truth: \\n \",hxTrue)\n",
    "print(\"Odom: \\n\", hxDR)\n",
    "print(\"SLAM: \\n\", x_opt)\n",
    "\n",
    "# performance will improve with more iterations, nodes and landmarks.\n",
    "print(\"\\ngraphSLAM localization error: \", np.sum((x_opt - hxTrue) ** 2))\n",
    "print(\"Odom localization error: \", np.sum((hxDR - hxTrue) ** 2))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### The references:\n",
    "\n",
    "\n",
    "* http://robots.stanford.edu/papers/thrun.graphslam.pdf\n",
    "\n",
    "* http://ais.informatik.uni-freiburg.de/teaching/ss13/robotics/slides/16-graph-slam.pdf\n",
    "\n",
    "* http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf\n",
    "\n",
    "N.B.\n",
    "An additional step is required that uses the estimated path to update the belief regarding the map."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.6.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
