4-Robot sensing.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 4. Robot sensing\n",
    "\n",
    "## Introduction\n",
    "\n",
    "In order to carry out tasks like localization or navigation, a mobile robot has to perceive its workspace. A variety of sensors can be used for that, as well as a number of probabilitic models for managing their behaviour."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## <span style=\"color:green\">OPTIONAL</span>\n",
    "\n",
    "<span style=\"color:green\">As commented, a number of sensors can be mounted on a mobile robot. In the robotic sensing lecture we discused some of the most popular ones. As an optional exercise, you can look for interesting information about any of them (or any one not listed below) and further describe it here to complete your knowledge.</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4.1 Sensors for autonomous robots\n",
    "\n",
    "### 4.1.1 Classification\n",
    "### 4.1.2 Beacons\n",
    "#### GPS\n",
    "### 4.1.3 Range sensors\n",
    "#### Sonar\n",
    "#### Infrared\n",
    "#### Laser scanner\n",
    "### 4.1.4 Cameras\n",
    "### 4.1.5 RGB-D cameras"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:green\">***END OF OPTIONAL PART***</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## 4.2 Probabilistic sensor models\n",
    "\n",
    "Typically, the sensors used onboard the robot do not deliver the exact truth of the quantities they are measuring, but a perturbed version. To model this behaviour, sensors' measurements are modelled by the probability distribution $p(z|v)$, where z models the measurmenet and v is the ground truth."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.2.1 Landmark-based models\n",
    "\n",
    "When the map consist of a collection of landmarks $m={m_i}, i=1,\\dots,N$, then the sensors used to provide observations of those landmarks measure:\n",
    "\n",
    "- **Distance/range** (*e.g.* radio, GPS, etc.): $z_i = d_i = h_i(x,m)+w_i$\n",
    "- **Bearing** (*e.g.* camera): $z_i = \\theta_i = h_i(x,m)+w_i$\n",
    "- **Distance/range and bearing** (*e.g.* stereo, features in a scan, etc.) $z_i = [d_i,\\theta_i]^T = h_i(x,m)+w_i$ *(in this case, $h_i(x,m)$ and $w_i$ are 2D vectors)*\n",
    "\n",
    "These types of maps and sensor measurements pose a new problem: **data association**, that is, with which landmark $m_i$ correspond the observation $z_i$ to: $h_i(x,m)=h(x,m_i)$. This problem is usually addressed by applying Chi-squared tests, although for the shake of simplicity in this book we will consider it as solved. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Playing with landmarks and robot poses\n",
    "\n",
    "In the remaining of this section we will familiarise ourselves with the process of observing landmarks from robots located at certain poses, as well as the transformations needed to make use of these observations, that is, to express those observations into the world frame and backwards.\n",
    "\n",
    "Some relevant concepts:\n",
    "\n",
    "- **World frame**: $x, y$ coordinates from a selected point of reference $(0, 0)$. We use to keep track of the robots pose, and within the map.\n",
    "- **Observation**: Information from the real world provided by a sensor, from the point of view (*pov*) of a certain robot.\n",
    "- **Range-bearing sensor**: Sensor model being used in this lesson. They detect how far is an object $(d)$ and its orientation relative to the robot's $(\\theta)$.\n",
    "\n",
    "The main tools for that are:\n",
    "\n",
    "- the composition of two poses.\n",
    "- the composition of a pose and a landmark.\n",
    "- the propagation of uncertainty through the Jacobians of these compositions."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#%matplotlib notebook\n",
    "%matplotlib inline\n",
    "\n",
    "# IMPORTS\n",
    "\n",
    "import math\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from scipy import stats\n",
    "\n",
    "from utils.PlotEllipse import PlotEllipse\n",
    "from utils.DrawRobot import DrawRobot\n",
    "from utils.tcomp import tcomp\n",
    "from utils.tinv import tinv, jac_tinv1 as jac_tinv\n",
    "from utils.Jacobians import J1, J2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We will address several problems of incremental complexity. The following figures will help you to follow the exercise. Concretely:\n",
    "\n",
    "1. We start with a robot ($R1$) placed at a perflectly known pose $p_1$ in the world frame (no uncertainty at this point), which observes a landmark with a range-bearing (polar) sensor affected by an error characterized with the covarince $W_{1p}$, providing a measurement $z_{1p}$ (In ipolar coordinates). **Our goal is to express the landmark position in the world frame**.\n",
    "2. In the second point, the robot pose is not perfectly known, but it follows a random variable (RV) so $p_1 \\sim N(\\overline{p}_1, \\Sigma_{p_1}).$. Again, **we have to express the landmark position in the world frame** and analyze what changes.\n",
    "3. Then, a second robot comes to play, and **we are asked to retrieve the relative pose $p_{12}$ between both robots**.\n",
    "4. At this point **we predict and obsevation from $R2$ to the landmark**. That is, using the range-bearing observation model and the information of the landmark position and uncertainty as provided by $R1$, we retrieve $z_{2p}$ expressed in polar coordiantes, as well as its associated uncertainty. \n",
    "5. Finally, $R2$ takes an observation of the landmark with its own sensor, so now we have two observations of the same landmark, one by $R1$ and another one by $R2$. **How could we combine these pieces of information?**\n",
    "\n",
    "\n",
    "\n",
    "<center>\n",
    "<img src=\"images/fig4-1-1.png\" width=\"800\">\n",
    "</center>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "1. **Express the observed landmark in coordinates of the world frame.** Let’s consider a robot R1 at a perfectly known pose $p_1 = [1, 2, 0.5]^T$ which observes\n",
    " a landmark $m$ with a range-bearing (polar) sensor affected by a zero-mean\n",
    " Gaussian error with covariance $W_{1p} = diag\\left([0.25, 0.04]\\right)$. The sensor provides the\n",
    " measurement $z_{1p} = [4m., 0.7rad.]^T$. \n",
    " \n",
    " Compute the Gaussian probability distribution  (mean and covariance) of the landmark observation in the world frame (the same as the robot)  and plot its corresponding ellipse (in magenta, $\\sigma=1$). Concretely, you have to complete the ``to_world_frame()`` function, and modify the demo code to show the ellipse representing the uncertainty. \n",
    " \n",
    " Consider the following:\n",
    " \n",
    " - You can express a sensor measurement in polar coordinates ($z_p=[r,\\alpha]^T$) as cartesian coordinates ($z_c=[z_x,z_y]^T)$ by:\n",
    " \n",
    " $$\n",
    "     z_c = \\begin{bmatrix} z_x \\\\ z_y \\end{bmatrix} \n",
    "         = \\begin{bmatrix} r \\ cos\\alpha \\\\ r \\ sin\\alpha \\end{bmatrix} \n",
    "         = f(r,\\alpha)\n",
    " $$\n",
    " \n",
    " - While computing the covariance of the landmark observation, you have to start by computing the covariance of the observation in the Cartesian robot $R1$ frame. That is:\n",
    " \n",
    " $$  \n",
    " W_{c} = \\frac{\\partial f(z_x,z_y)}{\\partial \\{r,\\alpha\\}} \\ W_{p} \\ \\frac{\\partial f(z_x,z_y)}{\\partial \\{r,\\alpha\\}}^T\n",
    " $$\n",
    "   \n",
    "   Then you can get the convariance in the world frame as:\n",
    "   \n",
    "   $$ W_{z\\_w} = \\frac{\\partial f(p,z_c)}{\\partial p} \\ Q_{p1\\_w} \\ \\left( \\frac{\\partial f(p,z_c)}{\\partial p} \\right)^T +\n",
    "        \\frac{\\partial f(p,z_c)}{\\partial z_c} \\ W_{c} \\ \\left( \\frac{\\partial f(p,z_c)}{\\partial z_c} \\right)^T\n",
    "   $$\n",
    "   \n",
    "   where $f(p,z_c) = p \\oplus z_c$, that is, the composition of the pose and the landmark.\n",
    "   \n",
    "   - Note that $\\frac{\\partial f(p,z_c)}{\\partial p}$ and $\\frac{\\partial f(p,z_c)}{\\partial z_c}$ are the same Jacobians as previously used to compose two poses in *robot motion*, but with a reduced size since **while working with landmarks the orientation is meaningless, only the position matters**. The functions ``J1()`` and ``J2()`` implement these jacobians for you.\n",
    "  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def to_world_frame(p1_w, Qp1_w, z1_p_r, W1):\n",
    "    \"\"\" Covert the observation z1_p_r to the world frame\n",
    "    \n",
    "        Args:\n",
    "            p1_w: Pose of the robot(in world frame)\n",
    "            Qp1_w: Covariance of the robot\n",
    "            z1_p_r: Observation to a landmark (polar coordinates) from robots perspective\n",
    "            W1: Covariance of the sensor in polar coordinates\n",
    "    \n",
    "        Returns:\n",
    "            z1_w: Pose of landmark in the world frame\n",
    "            Wz1: Covariance associated to z1_w\n",
    "    \"\"\"\n",
    "    \n",
    "    # Definition of useful variables\n",
    "    r, a = z1_p_r[0,0], z1_p_r[1,0]\n",
    "    s, c = np.sin(a), np.cos(a)\n",
    "\n",
    "    # Jacobian to convert the measurement uncertainty from polar to cartesian coordinates\n",
    "    Jac_pol_car = np.array([\n",
    "        [None, None],\n",
    "        [None, None]\n",
    "    ])\n",
    "\n",
    "    # Built a tuple with:\n",
    "    # z1_car_rel[0]: coordinates of the sensor measurement in cartesian coordinates relative to robot position\n",
    "    # z1_car_rel[1]: its associated uncertainty expressed in cartesian coordinates\n",
    "    z1_car_rel = (\n",
    "            np.vstack([None,None]), # position\n",
    "            None@W1@None # uncertainty\n",
    "            )\n",
    "    \n",
    "    z1_ext = np.vstack([z1_car_rel[0], 0]) # Extends z1 for its usage in the Jacobian functions J1 and J2\n",
    "\n",
    "    # Build the jacobians \n",
    "    Jac_ap = J1(p1_w ,z1_ext)[0:2,:] # Jacobian for expressing the uncertainty in the robot pose in a global frame\n",
    "    Jac_aa = J2(p1_w ,z1_ext)[0:2,0:2] # This one expresses the uncertainty in the measurment in a global frame\n",
    "    \n",
    "    z1_w = tcomp(None ,None)[0:2,[0]] # Compute coordinates of the landmark in the world\n",
    "    Wz1 = (None @ Qp1_w @ None\n",
    "          + None @ z1_car_rel[1] @ None) # Finally, propagate the covariance!\n",
    "    \n",
    "    return z1_w, Wz1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Robot\n",
    "p1_w = np.vstack([1, 2, 0.5]) # Robot R1 pose\n",
    "Qp1_w = np.zeros((3, 3)) # Robot pose convariance matrix (uncertainty)\n",
    "\n",
    "# Landmark observation\n",
    "z1_p_r = np.vstack([4., .7]) # Measurement/Observation\n",
    "W1 = np.diag([0.25, 0.04]) # Sensor noise covariance\n",
    "\n",
    "# Express the landmark observation in the world frame (mean and covariance)\n",
    "z1_w, Wz1 = to_world_frame(p1_w, Qp1_w, z1_p_r, W1)\n",
    "\n",
    "# Visualize the results\n",
    "fig, ax = plt.subplots()\n",
    "plt.xlim([-.5, 10])\n",
    "plt.ylim([-.5, 10])\n",
    "plt.grid()\n",
    "plt.tight_layout()\n",
    "\n",
    "DrawRobot(fig, ax, p1_w, label='R1', color='blue')\n",
    "    \n",
    "ax.plot(z1_w[0, 0], z1_w[1, 0], 'o', label='Z1', color='green')\n",
    "PlotEllipse(fig, ax, None, None, color='green')\n",
    "\n",
    "plt.legend()\n",
    "print('----\\tExercise 4.1.1\\t----\\n'+\n",
    "      'z1_w = {}\\'\\n'.format(z1_w.flatten())\n",
    "      + 'Wz1_w = \\n{}\\n'.format(Wz1))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:blue\">Expected results for demo:</span>\n",
    "\n",
    "\n",
    "```\n",
    "---- Exercise 4.1.1 ----\n",
    "z1_w = [2.44943102 5.72815634]'\n",
    "Wz1_w = \n",
    "[[ 0.58879177 -0.13171532]\n",
    " [-0.13171532  0.30120823]]\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "2. **Adding uncertainty to the robot posiion.** Now, let’s assume that the robot pose is not known, but it is a RV that follows a\n",
    "Gaussian probability distribution: $p_1 \\sim N([1, 2, 0.5]^T, \\Sigma_1)$ with $\\Sigma_1 = diag([0.08,0.6,\n",
    "0.02 ])$.\n",
    "\n",
    "    1. Compute the covariance matrix $\\Sigma_{m1}$ of the landmark in the world frame and\n",
    "     plot it as an ellipse centered at the mean $m_1$ (in blue, $sigma= 1$). Plot also\n",
    "     the covariance of the robot pose (in blue, $sigma= 1$).\n",
    "     \n",
    "    2. Compare the covariance with that obtained in the previous case. Is it\n",
    "     bigger? Is it bigger than that of the robot? Why?  **<span style=\"color:orangered\">Comment this in the Student discussion</span>**\n",
    "     \n",
    "     \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Robot\n",
    "p1_w = np.vstack([1, 2, 0.5]) # Robot R1 pose\n",
    "Qp1_w = np.diag([0.08,0.6,0.02])  # Robot pose convariance matrix (uncertainty)\n",
    "\n",
    "# Landmark observation\n",
    "z1_p_r = np.vstack([4., .7]) # Measurement/Observation\n",
    "W1 = np.diag([0.25, 0.04]) # Sensor noise covariance\n",
    "\n",
    "# Express the landmark observation in the world frame (mean and covariance)\n",
    "z1_w, Wz1 = to_world_frame(p1_w, Qp1_w, z1_p_r, W1)\n",
    "\n",
    "# MATPLOTLIB\n",
    "fig, ax = plt.subplots()\n",
    "plt.xlim([-.5, 10])\n",
    "plt.ylim([-.5, 10])\n",
    "plt.grid()\n",
    "plt.tight_layout()\n",
    "\n",
    "fig.canvas.draw()\n",
    "\n",
    "DrawRobot(fig, ax, p1_w, label='R1', color='red')  \n",
    "PlotEllipse(fig, ax, None, None, color='blue')\n",
    "\n",
    "ax.plot(z1_w[0, 0], z1_w[1, 0], 'o', label='Z1', color='green')\n",
    "PlotEllipse(fig, ax, None, None, color='green')\n",
    "\n",
    "plt.legend()\n",
    "print('---- Exercise 4.1.2 ----\\n'+\n",
    "      'Wz1_w = \\n{}\\n'.format(Wz1))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:blue\">Expected results for demo:</span>\n",
    "\n",
    "```\n",
    "---- Exercise 4.1.2 ----\n",
    "Wz1_w = \n",
    "[[ 0.94677477 -0.23978943]\n",
    " [-0.23978943  0.94322523]]\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "3. **Getting the relative pose between two robots**. Another robot `R2` is at pose $p_2 \\sim ([6m., 4m., 2.1rad.]^T, \\Sigma_2)$ with $\\Sigma_2 = diag([0.20,0.09,\n",
    " 0.03])$. Plot `p2` and its ellipse (covariance) in green ($sigma=1$). Compute the relative\n",
    " pose `p12` between `R1` and `R2`. \n",
    " \n",
    " This relative pose can be obtained in two different ways:\n",
    " - Through the composition of poses, but using $\\ominus p1$ instead of $p1$. Implement it in ``inverse_composition1()``.\n",
    " \n",
    " Mean: \n",
    " \n",
    " $$\n",
    " p12 = \\ominus p1 \\oplus p2 = f(\\ominus p1, p2) = \n",
    " \\begin{bmatrix} \n",
    "  x_{\\ominus p1} + x_{p2} cos \\theta_{\\ominus p1} - y_{p2} sin \\theta_{\\ominus p1} \\\\\n",
    "  y_{\\ominus p1} + x_{p2} sin \\theta_{\\ominus p1} + y_{p2} cos \\theta_{\\ominus p1} \\\\\n",
    "  \\theta_{\\ominus p1} + \\theta_{p2}\n",
    " \\end{bmatrix}\n",
    " $$\n",
    " \n",
    " Covariance:\n",
    " \n",
    " $$\n",
    " \\Sigma_{p12} = \\frac{\\partial p12}{\\partial \\ominus p1} \\frac{\\ominus p1}{\\partial p1} \\Sigma_{p1} \\frac{\\ominus p1}{\\partial p1}^T \\frac{\\partial p12}{\\partial \\ominus p1}^T \n",
    " +\n",
    " \\frac{\\partial p12}{\\partial p2} \\Sigma_{p2}  \\frac{\\partial p12}{\\partial p2}^T\n",
    " \\\\\n",
    " \\text{Applying the Chain rule} \\rightarrow \\Sigma_{p12} = \\frac{\\partial p12}{\\partial \\ominus p1} \\Sigma_{\\ominus p1} \\frac{\\partial p12}{\\partial \\ominus p1}^T\n",
    " +\n",
    " \\frac{\\partial p12}{\\partial p2} \\Sigma_{p2}  \\frac{\\partial p12}{\\partial p2}^T\n",
    " $$\n",
    " \n",
    " Being:\n",
    " \n",
    " $$\n",
    " \\frac{\\partial p12}{\\partial \\ominus p1} = \n",
    " \\begin{bmatrix}\n",
    " 1 & 0 & -x_{p2} sin \\theta_{\\ominus p1} - y_{p2} cos \\theta_{\\ominus p1} \\\\\n",
    " 0 & 1 & x_{p2} cos \\theta_{\\ominus p1} - y_{p2} sin \\theta_{\\ominus p1} \\\\\n",
    " 0 & 0 & 1\n",
    " \\end{bmatrix}  \n",
    " \\; \\; \\; \\; \\; \\frac{\\partial p12}{\\partial p2} = \n",
    " \\begin{bmatrix}\n",
    "cos \\theta_{\\ominus p1} & -sin \\theta_{\\ominus p1} & 0\\\\\n",
    "sin \\theta_{\\ominus p1} & cos \\theta_{\\ominus p1} & 0\\\\\n",
    " 0 & 0 & 1\n",
    " \\end{bmatrix}\n",
    " \\\\\n",
    " \\frac{\\partial \\ominus p1}{\\partial p1} = \n",
    " \\begin{bmatrix}\n",
    " -cos \\theta_{p1} & -sin \\theta_{p1} & x_{p1} sin \\theta_{p1} - y_{p1} cos \\theta_{p1} \\\\\n",
    " sin \\theta_{p1} & -cos \\theta_{p1} & x_{p1} cos \\theta_{p1} + y_{p1} sin \\theta_{p1}\\\\\n",
    " 0 & 0 & -1 \\\\\n",
    " \\end{bmatrix}\n",
    " \\; \\; \\; \\; \\; \\Sigma_{\\ominus p1} = \\frac{\\partial \\ominus p1}{\\partial p1} \\Sigma_{p1} \\frac{\\partial \\ominus p1}{\\partial p1}^T\n",
    " $$ \n",
    " \n",
    " - Using the inverse composition of poses. This one is given for you in ``inverse_composition2()``."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def inverse_composition1(p1_w, Qp1_w, p2_w, Qp2_w):\n",
    "    jac_inv_p = jac_tinv(p1_w)\n",
    "\n",
    "    inv_r1 = (\n",
    "        tinv(p1_w),\n",
    "        jac_inv_p @ Qp1_w @ jac_inv_p.T\n",
    "    )\n",
    "\n",
    "    jac_p12_inv = J1(inv_r1[0], p2_w)\n",
    "    jac_p12_p2 = J2(inv_r1[0], p2_w)\n",
    "\n",
    "    p12_w = tcomp(None, None)\n",
    "        \n",
    "    Qp12_w = (\n",
    "            None@inv_r1[1]@None \n",
    "            + None@Qp2_w@None\n",
    "        )\n",
    "    \n",
    "    return p12_w, Qp12_w"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def inverse_composition2(p1_w, Qp1_w, p2_w, Qp2_w):\n",
    "    dx, dy = p2_w[0, 0]-p1_w[0, 0], p2_w[1, 0]-p1_w[1, 0]\n",
    "    a = p2_w[2, 0] - p1_w[2, 0]\n",
    "    c, s = np.cos(p1_w[2, 0]), np.sin(p1_w[2, 0])\n",
    "\n",
    "    p12_w = np.array([\n",
    "        [dx*c + dy*s],\n",
    "        [-dx*s + dy*c],\n",
    "        [a]])\n",
    "    \n",
    "    jac_p12_r1 = np.array([\n",
    "        [-c, -s, -dx*s + dy*c],\n",
    "        [s, -c, -dx*c - dy*s],\n",
    "        [0, 0, -1]\n",
    "    ])\n",
    "\n",
    "    jac_p12_r2 = np.array([\n",
    "        [c, s, 0],\n",
    "        [-s, c, 0],\n",
    "        [0, 0, -1]\n",
    "    ])\n",
    "\n",
    "    #jac_p1_pinv = np.linalg.inv(jac_tinv(r1[0]))\n",
    "\n",
    "    Qp12_w = jac_p12_r1@Qp1_w@jac_p12_r1.T + jac_p12_r2@Qp2_w@jac_p12_r2.T\n",
    "\n",
    "    return p12_w, Qp12_w"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Robot R1\n",
    "p1_w = np.vstack([1., 2., 0.5])\n",
    "Qp1_w = np.diag([0.08, 0.6, 0.02])\n",
    "\n",
    "# Robot R2\n",
    "p2_w = np.vstack([6., 4., 2.1])\n",
    "Qp2_w = np.diag([0.20, 0.09, 0.03])\n",
    "\n",
    "# Obtain the relative pose p12 between both robots through the composition of poses\n",
    "p12_w, Qp12_w = inverse_composition1(p1_w, Qp1_w, p2_w, Qp2_w)\n",
    "print( '----\\tExercise 4.1.3 with method 1\\t----\\n'+\n",
    "        'p12_w = {}\\'\\n'.format(p12_w.flatten())+\n",
    "        'Qp12_w = \\n{}\\n'.format(Qp12_w))\n",
    "\n",
    "# Obtain the relative pose p12 between both robots through the inverse composition of poses\n",
    "p12_w, Qp12_w = inverse_composition2(p1_w, Qp1_w, p2_w, Qp2_w)\n",
    "print( '----\\tExercise 4.1.3 with method 2\\t----\\n'+\n",
    "        'p12_w = {}\\'\\n'.format(p12_w.flatten())+\n",
    "        'Qp12_w = \\n{}\\n'.format(Qp12_w))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:blue\">Expected results:</span>\n",
    " ```\n",
    " p12_w = [ 5.34676389 -0.64196257  1.6       ]'\n",
    " \n",
    " Qp12_w = \n",
    " [[0.38248035 0.24115    0.01283925]\n",
    " [0.24115    1.16751965 0.10693528]\n",
    " [0.01283925 0.10693528 0.05      ]]\n",
    " ```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "4. **Predicting an observation from the second robot.** According to the information, provided by R1, that we have about the position of the landmark m in\n",
    " the world coordinates (its location $z_{1\\_w}$ and its associated uncertainty $W_{z_1\\_w}$), compute the *predicted observation*\n",
    " distribution of $z_{2p} =[r, \\alpha] \\sim N(z_{2p}, W_{2p})$ as taken by a range-bearing sensor mounted on `R2`.\n",
    " Consider the following:\n",
    " \n",
    " - The range-bearing model for taking measurements is *(Notes: use ``np.arctan2()`` for computing the angle. At this point, ignore the noise $w_i$)*:\n",
    " \n",
    " $$\n",
    " z_i = \\begin{bmatrix} r_i \\\\ \\alpha_i \\end{bmatrix} = h(x,m_i) + w_i = \n",
    " \\begin{bmatrix} \\sqrt((x_i-x)^2+(y_i-y)^2) \\\\ atan(\\frac{y_i-y}{x_i-x}) - \\theta \\end{bmatrix} \n",
    " + w_i\n",
    " $$\n",
    " \n",
    " - We need to compute the covariance of the predicted observation in Polar  coordinates $(W_{2p})$. For that, use the following Jacobian: \n",
    " $$\n",
    "     \\frac{\\partial{p}}{\\partial{c}} = \n",
    "     \\begin{bmatrix}\n",
    "         \\cos{(\\alpha+\\theta)}  & \\sin{(\\alpha+\\theta)} \\\\\n",
    "         -\\sin{(\\alpha+\\theta)} / r  & \\cos{(\\alpha+\\theta)} / r\n",
    "     \\end{bmatrix}\n",
    " $$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def predicted_obs_from_pov(p1_w, z1_w, Wz1_w):\n",
    "    \"\"\" Method to translate a pose+covariance in the world frame to an observation.\n",
    "    \n",
    "        This method only translated the landmark to the pov of the robot.\n",
    "        It does not simulate a new observation.\n",
    "        \n",
    "        Args:\n",
    "            p1_w: Pose of the robot which acts as pov\n",
    "            z1_w: Landmark observed in cartesian coordinates(world frame)\n",
    "            Wz1_w: Covariance associated to the landmark.\n",
    "        Returns:\n",
    "            z2_pr: Expected observation of z1 from pov of p1_w\n",
    "            W2_p: Covariance associated to z2_pr\n",
    "    \"\"\"\n",
    "\n",
    "    # Take a measurement using the range-bearing model\n",
    "    z2_pr = np.vstack([\n",
    "            None, # distance\n",
    "            None # angle\n",
    "        ])\n",
    "      \n",
    "    # Jacobian from cartesian to polar at z2p_r when the covariance is in\n",
    "    # global coordianes\n",
    "    theta = z2_pr[1, 0] + p1_w[2, 0] \n",
    "    s, c = np.sin(theta), np.cos(theta)\n",
    "    r = z2_pr[0, 0]\n",
    "\n",
    "    Jac_car_pol = np.array([\n",
    "        [None, None],\n",
    "        [None, None]\n",
    "    ])\n",
    "\n",
    "    # Finally, propagate the uncertainty to polar coordinates in the\n",
    "    # robot frame\n",
    "    W2_p = None@None@None\n",
    "    \n",
    "    return z2_pr, W2_p"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "p2_w = np.vstack([6., 4., 2.1])\n",
    "\n",
    "z2_pr, W2_p = predicted_obs_from_pov(p2_w, z1_w, Wz1)\n",
    "print( '---- Exercise 4.1.4 ----\\n'+\n",
    "    'z2p_r = {}\\'\\n'.format(z2_pr.flatten())+\n",
    "    'W2_p = \\n{}\\n'.format(W2_p)    \n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:blue\">Expected output:</span>\n",
    "```\n",
    "---- Exercise 4.1.4 ----\n",
    "z2p_r = [3.94880545 0.58862004]'\n",
    "W2_p = \n",
    "[[ 1.13481128 -0.03710981]\n",
    " [-0.03710981  0.04843106]]\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "5. **Combining observations of the same landmark.** Assume now that a measurement $z_2 = [4 m., 0.3 rad.]^T$ of the landmark is taken\n",
    " from R2 with a sensor having the same precision as that of R1 ($W_{2p}= W_{1p}$)\n",
    " \n",
    " 1. What is the pdf of the observed landmark according to this observation? Plot the corresponding ellipse (in green, sigma=1).\n",
    " 2. Two different pdf’s are now associated to the same landmark.\n",
    "    1. Is that a contradiction?\n",
    "    2. Can you work out a solution that combines these two “pieces of\n",
    "     information”? Plot it in red. **<span style=\"color:orangered\">Comment this in the Student discussion</span>**\n",
    "     \n",
    "    \n",
    "<figure style=\"text-align:center\">\n",
    "  <img src=\"images/fig4-1-2.png\" width=\"400\" alt=\"\">\n",
    "  <figcaption>Fig. 2: Results from the last exercise </figcaption>\n",
    "</figure>    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def combine_pdfs(z1_w, Wz1_w, z2_w, Wz2_w):\n",
    "    \"\"\" Method to combine the pdfs associated with two observations of the same landmark.  \n",
    "        \n",
    "        Args:            \n",
    "            z1_w: Landmark observed in cartesian coordinates(world frame) from Robot 1\n",
    "            Wz1_w: Covariance associated to the landmark.\n",
    "            z1_w: Landmark observed in cartesian coordinates(world frame) from Robot 2\n",
    "            Wz2_w: Covariance associated to the landmark.\n",
    "        Returns:\n",
    "            z: Combined observation\n",
    "            W_z: Uncertainty associated to z\n",
    "    \"\"\"\n",
    "    None # Implement the needed code here\n",
    "\n",
    "    W_z = None\n",
    "    z = None\n",
    "\n",
    "    return z, W_z\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "z2_p_r = np.vstack([4., .3])\n",
    "Wz2_p_r = np.diag([0.25, 0.04])\n",
    "\n",
    "z1_w, Qz1 = to_world_frame(p1_w, Qp1_w, z1_p_r, W1)\n",
    "z2_w, Qz2 = to_world_frame(p2_w, Qp2_w, z2_p_r, W1)\n",
    "\n",
    "# Show results\n",
    "fig, ax = plt.subplots()\n",
    "plt.xlim([-.5, 10])\n",
    "plt.ylim([-.5, 10])\n",
    "plt.grid()\n",
    "plt.tight_layout()\n",
    "\n",
    "fig.canvas.draw()\n",
    "\n",
    "DrawRobot(fig, ax, p1_w, label='R1', color='blue')\n",
    "PlotEllipse(fig, ax, p1_w, Qp1_w, color='blue')\n",
    "\n",
    "DrawRobot(fig, ax, p2_w, label='R2', color='green')\n",
    "PlotEllipse(fig, ax, p2_w, Qp2_w, color='green')\n",
    "   \n",
    "ax.plot(z1_w[0, 0], z1_w[1, 0], 'o', label='Z1', color='blue')\n",
    "PlotEllipse(fig, ax, z1_w, Qz1, color='blue')\n",
    "          \n",
    "ax.plot(z2_w[0, 0], z2_w[1, 0], 'o', label='Z2', color='green')\n",
    "PlotEllipse(fig, ax, z2_w, Qz2, color='green')\n",
    "\n",
    "z_w, Wz_w = combine_pdfs(z1_w, Qz1, z2_w, Qz2)\n",
    "ax.plot(z_w[0, 0], z_w[1, 0], 'o', label='Z3', color='red')\n",
    "PlotEllipse(fig, ax, z_w, Wz_w, color='red')\n",
    "          \n",
    "plt.legend()\n",
    "\n",
    "# Print results\n",
    "print( '----\\tExercise 4.1.5\\t----\\n'+\n",
    "    'z2_w = {}\\'\\n'.format(z2_w.flatten())+\n",
    "    'Qz2 = \\n{}\\n'.format(Qz2)\n",
    "    )\n",
    "\n",
    "# Print results\n",
    "print( '----\\tExercise 4.1.5 parte 2\\t----\\n'+\n",
    "    'z_w = {}\\'\\n'.format(z_w.flatten())+\n",
    "    'Wz_w = \\n{}\\n'.format(Wz_w)\n",
    "    )"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:blue\">Expected ouputs:</span>\n",
    "\n",
    "### Sensor measurement from R2\n",
    "\n",
    "```\n",
    "z2_w = [3.05042514 6.70185272]'\n",
    "Qz2 = \n",
    "[[0.84693794 0.4333316 ]\n",
    " [0.4333316  0.81306206]]\n",
    "```\n",
    "\n",
    "### Combined information\n",
    "```\n",
    "---- Exercise 4.1.5 parte 2 ----\n",
    "z_w = [2.58757252 6.15534036]'\n",
    "Wz_w = \n",
    "[[0.37966125 0.07773125]\n",
    " [0.07773125 0.36999739]]\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## <span style=\"color:green\">OPTIONAL</span>\n",
    "\n",
    "<span style=\"color:green\">An alternative to *landmark observation models* are *scan observation* ones, which work with scan-based sensors. Below, the three most popular ones are listed. Surf the internet for some code illustrating any of them, and include it in the notebook with a brief description of how it works and its purpose.</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4.2.2 Scan observation models\n",
    "\n",
    "Scan observation models are used when the sensor mounted on the robot provides a scan measuring distance and angle to obstacles in the workspace, *e.g.* a laser range finder. In this case, each element in the map is a cell described by its position (and probably a color representing if its free of obstacles or occupied), and data association is not explicitly addressed. \n",
    "\n",
    "#### Beam model\n",
    "#### Likelihood field\n",
    "#### Scan matching"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:green\">***END OF OPTIONAL PART***</span>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Student discussion\n",
    "In the cell below, discuss what has been done in the notebook, what you have found interesting, or any other relevant thought."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<span style=\"color:orangered\">***Write your answer here***</span>"
   ]
  }
 ],
 "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.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}