3. Landmark Detection and Tracking.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Project 3:  Implement SLAM \n",
    "\n",
    "---\n",
    "\n",
    "## Project Overview\n",
    "\n",
    "In this project, you'll implement SLAM for robot that moves and senses in a 2 dimensional, grid world!\n",
    "\n",
    "SLAM gives us a way to both localize a robot and build up a map of its environment as a robot moves and senses in real-time. This is an active area of research in the fields of robotics and autonomous systems. Since this localization and map-building relies on the visual sensing of landmarks, this is a computer vision problem. \n",
    "\n",
    "Using what you've learned about robot motion, representations of uncertainty in motion and sensing, and localization techniques, you will be tasked with defining a function, `slam`, which takes in six parameters as input and returns the vector `mu`. \n",
    "> `mu` contains the (x,y) coordinate locations of the robot as it moves, and the positions of landmarks that it senses in the world\n",
    "\n",
    "You can implement helper functions as you see fit, but your function must return `mu`. The vector, `mu`, should have (x, y) coordinates interlaced, for example, if there were 2 poses and 2 landmarks, `mu` will look like the following, where `P` is the robot position and `L` the landmark position:\n",
    "```\n",
    "mu =  matrix([[Px0],\n",
    "              [Py0],\n",
    "              [Px1],\n",
    "              [Py1],\n",
    "              [Lx0],\n",
    "              [Ly0],\n",
    "              [Lx1],\n",
    "              [Ly1]])\n",
    "```\n",
    "\n",
    "You can see that `mu` holds the poses first `(x0, y0), (x1, y1), ...,` then the landmark locations at the end of the matrix; we consider a `nx1` matrix to be a vector.\n",
    "\n",
    "## Generating an environment\n",
    "\n",
    "In a real SLAM problem, you may be given a map that contains information about landmark locations, and in this example, we will make our own data using the `make_data` function, which generates a world grid with landmarks in it and then generates data by placing a robot in that world and moving and sensing over some numer of time steps. The `make_data` function relies on a correct implementation of robot move/sense functions, which, at this point, should be complete and in the `robot_class.py` file. The data is collected as an instantiated robot moves and senses in a world. Your SLAM function will take in this data as input. So, let's first create this data and explore how it represents the movement and sensor measurements that our robot takes.\n",
    "\n",
    "---"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Create the world\n",
    "\n",
    "Use the code below to generate a world of a specified size with randomly generated landmark locations. You can change these parameters and see how your implementation of SLAM responds! \n",
    "\n",
    "`data` holds the sensors measurements and motion of your robot over time. It stores the measurements as `data[i][0]` and the motion as `data[i][1]`.\n",
    "\n",
    "#### Helper functions\n",
    "\n",
    "You will be working with the `robot` class that may look familiar from the first notebook, \n",
    "\n",
    "In fact, in the `helpers.py` file, you can read the details of how data is made with the `make_data` function. It should look very similar to the robot move/sense cycle you've seen in the first notebook."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from helpers import make_data\n",
    "\n",
    "# your implementation of slam should work with the following inputs\n",
    "# feel free to change these input values and see how it responds!\n",
    "\n",
    "# world parameters\n",
    "num_landmarks      = 5        # number of landmarks\n",
    "N                  = 20       # time steps\n",
    "world_size         = 100.0    # size of world (square)\n",
    "\n",
    "# robot parameters\n",
    "measurement_range  = 50.0     # range at which we can sense landmarks\n",
    "motion_noise       = 2.0      # noise in robot motion\n",
    "measurement_noise  = 2.0      # noise in the measurements\n",
    "distance           = 20.0     # distance by which robot (intends to) move each iteratation \n",
    "\n",
    "\n",
    "# make_data instantiates a robot, AND generates random landmarks for a given world size and number of landmarks\n",
    "data = make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### A note on `make_data`\n",
    "\n",
    "The function above, `make_data`, takes in so many world and robot motion/sensor parameters because it is responsible for:\n",
    "1. Instantiating a robot (using the robot class)\n",
    "2. Creating a grid world with landmarks in it\n",
    "\n",
    "**This function also prints out the true location of landmarks and the *final* robot location, which you should refer back to when you test your implementation of SLAM.**\n",
    "\n",
    "The `data` this returns is an array that holds information about **robot sensor measurements** and **robot motion** `(dx, dy)` that is collected over a number of time steps, `N`. You will have to use *only* these readings about motion and measurements to track a robot over time and find the determine the location of the landmarks using SLAM. We only print out the true landmark locations for comparison, later.\n",
    "\n",
    "\n",
    "In `data` the measurement and motion data can be accessed from the first and second index in the columns of the data array. See the following code for an example, where `i` is the time step:\n",
    "```\n",
    "measurement = data[i][0]\n",
    "motion = data[i][1]\n",
    "```\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# print out some stats about the data\n",
    "time_step = 0\n",
    "\n",
    "print('Example measurements: \\n', data[time_step][0])\n",
    "print('\\n')\n",
    "print('Example motion: \\n', data[time_step][1])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Try changing the value of `time_step`, you should see that the list of measurements varies based on what in the world the robot sees after it moves. As you know from the first notebook, the robot can only sense so far and with a certain amount of accuracy in the measure of distance between its location and the location of landmarks. The motion of the robot always is a vector with two values: one for x and one for y displacement. This structure will be useful to keep in mind as you traverse this data in your implementation of slam."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Initialize Constraints\n",
    "\n",
    "One of the most challenging tasks here will be to create and modify the constraint matrix and vector: omega and xi. In the second notebook, you saw an example of how omega and xi could hold all the values the define the relationships between robot poses `xi` and landmark positions `Li` in a 1D world, as seen below, where omega is the blue matrix and xi is the pink vector.\n",
    "\n",
    "<img src='images/motion_constraint.png' width=50% height=50% />\n",
    "\n",
    "\n",
    "In *this* project, you are tasked with implementing constraints for a 2D world. We are referring to robot poses as `Px, Py` and landmark positions as `Lx, Ly`, and one way to approach this challenge is to add *both* x and y locations in the constraint matrices.\n",
    "\n",
    "<img src='images/constraints2D.png' width=50% height=50% />\n",
    "\n",
    "You may also choose to create two of each omega and xi (one for x and one for y positions)."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### TODO: Write a function that initializes omega and xi\n",
    "\n",
    "Complete the function `initialize_constraints` so that it returns `omega` and `xi` constraints for the starting position of the robot. Any values that we do not yet know should be initialized with the value `0`. You may assume that our robot starts out in exactly the middle of the world with 100% confidence (no motion or measurement noise at this point). The inputs `N` time steps, `num_landmarks`, and `world_size` should give you all the information you need to construct intial constraints of the correct size and starting values.\n",
    "\n",
    "*Depending on your approach you may choose to return one omega and one xi that hold all (x,y) positions *or* two of each (one for x values and one for y); choose whichever makes most sense to you!*"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def initialize_constraints(N, num_landmarks, world_size):\n",
    "    ''' This function takes in a number of time steps N, number of landmarks, and a world_size,\n",
    "        and returns initialized constraint matrices, omega and xi.'''\n",
    "    \n",
    "    ## Recommended: Define and store the size (rows/cols) of the constraint matrix in a variable\n",
    "    \n",
    "    ## TODO: Define the constraint matrix, Omega, with two initial \"strength\" values\n",
    "    ## for the initial x, y location of our robot\n",
    "    omega = [0]\n",
    "    \n",
    "    ## TODO: Define the constraint *vector*, xi\n",
    "    ## you can assume that the robot starts out in the middle of the world with 100% confidence\n",
    "    xi = [0]\n",
    "    \n",
    "    return omega, xi\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Test as you go\n",
    "\n",
    "It's good practice to test out your code, as you go. Since `slam` relies on creating and updating constraint matrices, `omega` and `xi` to account for robot sensor measurements and motion, let's check that they initialize as expected for any given parameters.\n",
    "\n",
    "Below, you'll find some test code that allows you to visualize the results of your function `initialize_constraints`. We are using the [seaborn](https://seaborn.pydata.org/) library for visualization.\n",
    "\n",
    "**Please change the test values of N, landmarks, and world_size and see the results**. Be careful not to use these values as input into your final smal function.\n",
    "\n",
    "This code assumes that you have created one of each constraint: `omega` and `xi`, but you can change and add to this code, accordingly. The constraints should vary in size with the number of time steps and landmarks as these values affect the number of poses a robot will take `(Px0,Py0,...Pxn,Pyn)` and landmark locations `(Lx0,Ly0,...Lxn,Lyn)` whose relationships should be tracked in the constraint matrices. Recall that `omega` holds the weights of each variable and `xi` holds the value of the sum of these variables, as seen in Notebook 2. You'll need the `world_size` to determine the starting pose of the robot in the world and fill in the initial values for `xi`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# import data viz resources\n",
    "import matplotlib.pyplot as plt\n",
    "from pandas import DataFrame\n",
    "import seaborn as sns\n",
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# define a small N and world_size (small for ease of visualization)\n",
    "N_test = 5\n",
    "num_landmarks_test = 2\n",
    "small_world = 10\n",
    "\n",
    "# initialize the constraints\n",
    "initial_omega, initial_xi = initialize_constraints(N_test, num_landmarks_test, small_world)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# define figure size\n",
    "plt.rcParams[\"figure.figsize\"] = (10,7)\n",
    "\n",
    "# display omega\n",
    "sns.heatmap(DataFrame(initial_omega), cmap='Blues', annot=True, linewidths=.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# define  figure size\n",
    "plt.rcParams[\"figure.figsize\"] = (1,7)\n",
    "\n",
    "# display xi\n",
    "sns.heatmap(DataFrame(initial_xi), cmap='Oranges', annot=True, linewidths=.5)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "---\n",
    "## SLAM inputs \n",
    "\n",
    "In addition to `data`, your slam function will also take in:\n",
    "* N -   The number of time steps that a robot will be moving and sensing\n",
    "* num_landmarks - The number of landmarks in the world\n",
    "* world_size - The size (w/h) of your world\n",
    "* motion_noise - The noise associated with motion; the update confidence for motion should be `1.0/motion_noise`\n",
    "* measurement_noise - The noise associated with measurement/sensing; the update weight for measurement should be `1.0/measurement_noise`\n",
    "\n",
    "#### A note on noise\n",
    "\n",
    "Recall that `omega` holds the relative \"strengths\" or weights for each position variable, and you can update these weights by accessing the correct index in omega `omega[row][col]` and *adding/subtracting* `1.0/noise` where `noise` is measurement or motion noise. `Xi` holds actual position values, and so to update `xi` you'll do a similar addition process only using the actual value of a motion or measurement. So for a vector index `xi[row][0]` you will end up adding/subtracting one measurement or motion divided by their respective `noise`.\n",
    "\n",
    "### TODO: Implement Graph SLAM\n",
    "\n",
    "Follow the TODO's below to help you complete this slam implementation (these TODO's are in the recommended order), then test out your implementation! \n",
    "\n",
    "#### Updating with motion and measurements\n",
    "\n",
    "With a 2D omega and xi structure as shown above (in earlier cells), you'll have to be mindful about how you update the values in these constraint matrices to account for motion and measurement constraints in the x and y directions. Recall that the solution to these matrices (which holds all values for robot poses `P` and landmark locations `L`) is the vector, `mu`, which can be computed at the end of the construction of omega and xi as the inverse of omega times xi: $\\mu = \\Omega^{-1}\\xi$\n",
    "\n",
    "**You may also choose to return the values of `omega` and `xi` if you want to visualize their final state!**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "## TODO: Complete the code to implement SLAM\n",
    "\n",
    "## slam takes in 6 arguments and returns mu, \n",
    "## mu is the entire path traversed by a robot (all x,y poses) *and* all landmarks locations\n",
    "def slam(data, N, num_landmarks, world_size, motion_noise, measurement_noise):\n",
    "    \n",
    "    ## TODO: Use your initilization to create constraint matrices, omega and xi\n",
    "    \n",
    "    ## TODO: Iterate through each time step in the data\n",
    "    ## get all the motion and measurement data as you iterate\n",
    "            \n",
    "    ## TODO: update the constraint matrix/vector to account for all *measurements*\n",
    "    ## this should be a series of additions that take into account the measurement noise\n",
    "            \n",
    "    ## TODO: update the constraint matrix/vector to account for all *motion* and motion noise\n",
    "    \n",
    "    ## TODO: After iterating through all the data\n",
    "    ## Compute the best estimate of poses and landmark positions\n",
    "    ## using the formula, omega_inverse * Xi\n",
    "    mu = None\n",
    "    \n",
    "    return mu # return `mu`\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Helper functions\n",
    "\n",
    "To check that your implementation of SLAM works for various inputs, we have provided two helper functions that will help display the estimated pose and landmark locations that your function has produced. First, given a result `mu` and number of time steps, `N`, we define a function that extracts the poses and landmarks locations and returns those as their own, separate lists. \n",
    "\n",
    "Then, we define a function that nicely print out these lists; both of these we will call, in the next step.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# a helper function that creates a list of poses and of landmarks for ease of printing\n",
    "# this only works for the suggested constraint architecture of interlaced x,y poses\n",
    "def get_poses_landmarks(mu, N):\n",
    "    # create a list of poses\n",
    "    poses = []\n",
    "    for i in range(N):\n",
    "        poses.append((mu[2*i].item(), mu[2*i+1].item()))\n",
    "\n",
    "    # create a list of landmarks\n",
    "    landmarks = []\n",
    "    for i in range(num_landmarks):\n",
    "        landmarks.append((mu[2*(N+i)].item(), mu[2*(N+i)+1].item()))\n",
    "\n",
    "    # return completed lists\n",
    "    return poses, landmarks\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def print_all(poses, landmarks):\n",
    "    print('\\n')\n",
    "    print('Estimated Poses:')\n",
    "    for i in range(len(poses)):\n",
    "        print('['+', '.join('%.3f'%p for p in poses[i])+']')\n",
    "    print('\\n')\n",
    "    print('Estimated Landmarks:')\n",
    "    for i in range(len(landmarks)):\n",
    "        print('['+', '.join('%.3f'%l for l in landmarks[i])+']')\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Run SLAM\n",
    "\n",
    "Once you've completed your implementation of `slam`, see what `mu` it returns for different world sizes and different landmarks!\n",
    "\n",
    "### What to Expect\n",
    "\n",
    "The `data` that is generated is random, but you did specify the number, `N`, or time steps that the robot was expected to move and the `num_landmarks` in the world (which your implementation of `slam` should see and estimate a position for. Your robot should also start with an estimated pose in the very center of your square world, whose size is defined by `world_size`.\n",
    "\n",
    "With these values in mind, you should expect to see a result that displays two lists:\n",
    "1. **Estimated poses**, a list of (x, y) pairs that is exactly `N` in length since this is how many motions your robot has taken. The very first pose should be the center of your world, i.e. `[50.000, 50.000]` for a world that is 100.0 in square size.\n",
    "2. **Estimated landmarks**, a list of landmark positions (x, y) that is exactly `num_landmarks` in length. \n",
    "\n",
    "#### Landmark Locations\n",
    "\n",
    "If you refer back to the printout of *exact* landmark locations when this data was created, you should see values that are very similar to those coordinates, but not quite (since `slam` must account for noise in motion and measurement)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# call your implementation of slam, passing in the necessary parameters\n",
    "mu = slam(data, N, num_landmarks, world_size, motion_noise, measurement_noise)\n",
    "\n",
    "# print out the resulting landmarks and poses\n",
    "if(mu is not None):\n",
    "    # get the lists of poses and landmarks\n",
    "    # and print them out\n",
    "    poses, landmarks = get_poses_landmarks(mu, N)\n",
    "    print_all(poses, landmarks)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Visualize the constructed world\n",
    "\n",
    "Finally, using the `display_world` code from the `helpers.py` file (which was also used in the first notebook), we can actually visualize what you have coded with `slam`: the final position of the robot and the positon of landmarks, created from only motion and measurement data!\n",
    "\n",
    "**Note that these should be very similar to the printed *true* landmark locations and final pose from our call to `make_data` early in this notebook.**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# import the helper function\n",
    "from helpers import display_world\n",
    "\n",
    "# Display the final world!\n",
    "\n",
    "# define figure size\n",
    "plt.rcParams[\"figure.figsize\"] = (20,20)\n",
    "\n",
    "# check if poses has been created\n",
    "if 'poses' in locals():\n",
    "    # print out the last pose\n",
    "    print('Last pose: ', poses[-1])\n",
    "    # display the last position of the robot *and* the landmark positions\n",
    "    display_world(int(world_size), poses[-1], landmarks)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Question: How far away is your final pose (as estimated by `slam`) compared to the *true* final pose? Why do you think these poses are different?\n",
    "\n",
    "You can find the true value of the final pose in one of the first cells where `make_data` was called. You may also want to look at the true landmark locations and compare them to those that were estimated by `slam`. Ask yourself: what do you think would happen if we moved and sensed more (increased N)? Or if we had lower/higher noise parameters."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "**Answer**: (Write your answer here.)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Testing\n",
    "\n",
    "To confirm that your slam code works before submitting your project, it is suggested that you run it on some test data and cases. A few such cases have been provided for you, in the cells below. When you are ready, uncomment the test cases in the next cells (there are two test cases, total); your output should be **close-to or exactly** identical to the given results. If there are minor discrepancies it could be a matter of floating point accuracy or in the calculation of the inverse matrix.\n",
    "\n",
    "### Submit your project\n",
    "\n",
    "If you pass these tests, it is a good indication that your project will pass all the specifications in the project rubric. Follow the submission instructions to officially submit!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Here is the data and estimated outputs for test case 1\n",
    "\n",
    "test_data1 = [[[[1, 19.457599255548065, 23.8387362100849], [2, -13.195807561967236, 11.708840328458608], [3, -30.0954905279171, 15.387879242505843]], [-12.2607279422326, -15.801093326936487]], [[[2, -0.4659930049620491, 28.088559771215664], [4, -17.866382374890936, -16.384904503932]], [-12.2607279422326, -15.801093326936487]], [[[4, -6.202512900833806, -1.823403210274639]], [-12.2607279422326, -15.801093326936487]], [[[4, 7.412136480918645, 15.388585962142429]], [14.008259661173426, 14.274756084260822]], [[[4, -7.526138813444998, -0.4563942429717849]], [14.008259661173426, 14.274756084260822]], [[[2, -6.299793150150058, 29.047830407717623], [4, -21.93551130411791, -13.21956810989039]], [14.008259661173426, 14.274756084260822]], [[[1, 15.796300959032276, 30.65769689694247], [2, -18.64370821983482, 17.380022987031367]], [14.008259661173426, 14.274756084260822]], [[[1, 0.40311325410337906, 14.169429532679855], [2, -35.069349468466235, 2.4945558982439957]], [14.008259661173426, 14.274756084260822]], [[[1, -16.71340983241936, -2.777000269543834]], [-11.006096015782283, 16.699276945166858]], [[[1, -3.611096830835776, -17.954019226763958]], [-19.693482634035977, 3.488085684573048]], [[[1, 18.398273354362416, -22.705102332550947]], [-19.693482634035977, 3.488085684573048]], [[[2, 2.789312482883833, -39.73720193121324]], [12.849049222879723, -15.326510824972983]], [[[1, 21.26897046581808, -10.121029799040915], [2, -11.917698965880655, -23.17711662602097], [3, -31.81167947898398, -16.7985673023331]], [12.849049222879723, -15.326510824972983]], [[[1, 10.48157743234859, 5.692957082575485], [2, -22.31488473554935, -5.389184118551409], [3, -40.81803984305378, -2.4703329790238118]], [12.849049222879723, -15.326510824972983]], [[[0, 10.591050242096598, -39.2051798967113], [1, -3.5675572049297553, 22.849456408289125], [2, -38.39251065320351, 7.288990306029511]], [12.849049222879723, -15.326510824972983]], [[[0, -3.6225556479370766, -25.58006865235512]], [-7.8874682868419965, -18.379005523261092]], [[[0, 1.9784503557879374, -6.5025974151499]], [-7.8874682868419965, -18.379005523261092]], [[[0, 10.050665232782423, 11.026385307998742]], [-17.82919359778298, 9.062000642947142]], [[[0, 26.526838150174818, -0.22563393232425621], [4, -33.70303936886652, 2.880339841013677]], [-17.82919359778298, 9.062000642947142]]]\n",
    "\n",
    "##  Test Case 1\n",
    "##\n",
    "# Estimated Pose(s):\n",
    "#     [50.000, 50.000]\n",
    "#     [37.858, 33.921]\n",
    "#     [25.905, 18.268]\n",
    "#     [13.524, 2.224]\n",
    "#     [27.912, 16.886]\n",
    "#     [42.250, 30.994]\n",
    "#     [55.992, 44.886]\n",
    "#     [70.749, 59.867]\n",
    "#     [85.371, 75.230]\n",
    "#     [73.831, 92.354]\n",
    "#     [53.406, 96.465]\n",
    "#     [34.370, 100.134]\n",
    "#     [48.346, 83.952]\n",
    "#     [60.494, 68.338]\n",
    "#     [73.648, 53.082]\n",
    "#     [86.733, 38.197]\n",
    "#     [79.983, 20.324]\n",
    "#     [72.515, 2.837]\n",
    "#     [54.993, 13.221]\n",
    "#     [37.164, 22.283]\n",
    "\n",
    "\n",
    "# Estimated Landmarks:\n",
    "#     [82.679, 13.435]\n",
    "#     [70.417, 74.203]\n",
    "#     [36.688, 61.431]\n",
    "#     [18.705, 66.136]\n",
    "#     [20.437, 16.983]\n",
    "\n",
    "\n",
    "### Uncomment the following three lines for test case 1 and compare the output to the values above ###\n",
    "\n",
    "# mu_1 = slam(test_data1, 20, 5, 100.0, 2.0, 2.0)\n",
    "# poses, landmarks = get_poses_landmarks(mu_1, 20)\n",
    "# print_all(poses, landmarks)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "# Here is the data and estimated outputs for test case 2\n",
    "\n",
    "test_data2 = [[[[0, 26.543274387283322, -6.262538160312672], [3, 9.937396825799755, -9.128540360867689]], [18.92765331253674, -6.460955043986683]], [[[0, 7.706544739722961, -3.758467215445748], [1, 17.03954411948937, 31.705489938553438], [3, -11.61731288777497, -6.64964096716416]], [18.92765331253674, -6.460955043986683]], [[[0, -12.35130507136378, 2.585119104239249], [1, -2.563534536165313, 38.22159657838369], [3, -26.961236804740935, -0.4802312626141525]], [-11.167066095509824, 16.592065417497455]], [[[0, 1.4138633151721272, -13.912454837810632], [1, 8.087721200818589, 20.51845934354381], [3, -17.091723454402302, -16.521500551709707], [4, -7.414211721400232, 38.09191602674439]], [-11.167066095509824, 16.592065417497455]], [[[0, 12.886743222179561, -28.703968411636318], [1, 21.660953298391387, 3.4912891084614914], [3, -6.401401414569506, -32.321583037341625], [4, 5.034079343639034, 23.102207946092893]], [-11.167066095509824, 16.592065417497455]], [[[1, 31.126317672358578, -10.036784369535214], [2, -38.70878528420893, 7.4987265861424595], [4, 17.977218575473767, 6.150889254289742]], [-6.595520680493778, -18.88118393939265]], [[[1, 41.82460922922086, 7.847527392202475], [3, 15.711709540417502, -30.34633659912818]], [-6.595520680493778, -18.88118393939265]], [[[0, 40.18454208294434, -6.710999804403755], [3, 23.019508919299156, -10.12110867290604]], [-6.595520680493778, -18.88118393939265]], [[[3, 27.18579315312821, 8.067219022708391]], [-6.595520680493778, -18.88118393939265]], [[], [11.492663265706092, 16.36822198838621]], [[[3, 24.57154567653098, 13.461499960708197]], [11.492663265706092, 16.36822198838621]], [[[0, 31.61945290413707, 0.4272295085799329], [3, 16.97392299158991, -5.274596836133088]], [11.492663265706092, 16.36822198838621]], [[[0, 22.407381798735177, -18.03500068379259], [1, 29.642444125196995, 17.3794951934614], [3, 4.7969752441371645, -21.07505361639969], [4, 14.726069092569372, 32.75999422300078]], [11.492663265706092, 16.36822198838621]], [[[0, 10.705527984670137, -34.589764174299596], [1, 18.58772336795603, -0.20109708164787765], [3, -4.839806195049413, -39.92208742305105], [4, 4.18824810165454, 14.146847823548889]], [11.492663265706092, 16.36822198838621]], [[[1, 5.878492140223764, -19.955352450942357], [4, -7.059505455306587, -0.9740849280550585]], [19.628527845173146, 3.83678180657467]], [[[1, -11.150789592446378, -22.736641053247872], [4, -28.832815721158255, -3.9462962046291388]], [-19.841703647091965, 2.5113335861604362]], [[[1, 8.64427397916182, -20.286336970889053], [4, -5.036917727942285, -6.311739993868336]], [-5.946642674882207, -19.09548221169787]], [[[0, 7.151866679283043, -39.56103232616369], [1, 16.01535401373368, -3.780995345194027], [4, -3.04801331832137, 13.697362774960865]], [-5.946642674882207, -19.09548221169787]], [[[0, 12.872879480504395, -19.707592098123207], [1, 22.236710716903136, 16.331770792606406], [3, -4.841206109583004, -21.24604435851242], [4, 4.27111163223552, 32.25309748614184]], [-5.946642674882207, -19.09548221169787]]] \n",
    "\n",
    "\n",
    "##  Test Case 2\n",
    "##\n",
    "# Estimated Pose(s):\n",
    "#     [50.000, 50.000]\n",
    "#     [69.035, 45.061]\n",
    "#     [87.655, 38.971]\n",
    "#     [76.084, 55.541]\n",
    "#     [64.283, 71.684]\n",
    "#     [52.396, 87.887]\n",
    "#     [44.674, 68.948]\n",
    "#     [37.532, 49.680]\n",
    "#     [31.392, 30.893]\n",
    "#     [24.796, 12.012]\n",
    "#     [33.641, 26.440]\n",
    "#     [43.858, 43.560]\n",
    "#     [54.735, 60.659]\n",
    "#     [65.884, 77.791]\n",
    "#     [77.413, 94.554]\n",
    "#     [96.740, 98.020]\n",
    "#     [76.149, 99.586]\n",
    "#     [70.211, 80.580]\n",
    "#     [64.130, 61.270]\n",
    "#     [58.183, 42.175]\n",
    "\n",
    "\n",
    "# Estimated Landmarks:\n",
    "#     [76.777, 42.415]\n",
    "#     [85.109, 76.850]\n",
    "#     [13.687, 95.386]\n",
    "#     [59.488, 39.149]\n",
    "#     [69.283, 93.654]\n",
    "\n",
    "\n",
    "### Uncomment the following three lines for test case 2 and compare to the values above ###\n",
    "\n",
    "# mu_2 = slam(test_data2, 20, 5, 100.0, 2.0, 2.0)\n",
    "# poses, landmarks = get_poses_landmarks(mu_2, 20)\n",
    "# print_all(poses, landmarks)\n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python [conda env:cvnd_1]",
   "language": "python",
   "name": "conda-env-cvnd_1-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.5.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}