{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## LIDAR to 2D grid map example\n",
    "\n",
    "This simple tutorial shows how to read LIDAR (range) measurements from a file and convert it to occupancy grid.\n",
    "\n",
    "Occupancy grid maps (_Hans Moravec, A.E. Elfes: High resolution maps from wide angle sonar, Proc. IEEE Int. Conf. Robotics Autom. (1985)_) are a popular, probabilistic approach to represent the environment. The grid is basically discrete representation of the environment, which shows if a grid cell is occupied or not. Here the map is represented as a `numpy array`, and numbers close to 1 means the cell is occupied (_marked with red on the next image_), numbers close to 0 means they are free (_marked with green_). The grid has the ability to represent unknown (unobserved) areas, which are close to 0.5.\n",
    "\n",
    "![Example](grid_map_example.png)\n",
    "\n",
    "\n",
    "In order to construct the grid map from the measurement we need to discretise the values. But, first let's need to `import` some necessary packages."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from math import cos, sin, radians, pi"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The measurement file contains the distances and the corresponding angles in a `csv` (comma separated values) format. Let's write the `file_read` method:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def file_read(f):\n",
    "    \"\"\"\n",
    "    Reading LIDAR laser beams (angles and corresponding distance data)\n",
    "    \"\"\"\n",
    "    measures = [line.split(\",\") for line in open(f)]\n",
    "    angles = []\n",
    "    distances = []\n",
    "    for measure in measures:\n",
    "        angles.append(float(measure[0]))\n",
    "        distances.append(float(measure[1]))\n",
    "    angles = np.array(angles)\n",
    "    distances = np.array(distances)\n",
    "    return angles, distances"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "From the distances and the angles it is easy to determine the `x` and `y` coordinates with `sin` and `cos`. \n",
    "In order to display it `matplotlib.pyplot` (`plt`) is used."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 162408 characters]\n",
      "text/plain": [
       "<Figure size 432x720 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "ang, dist = file_read(\"lidar01.csv\")\n",
    "ox = np.sin(ang) * dist\n",
    "oy = np.cos(ang) * dist\n",
    "plt.figure(figsize=(6,10))\n",
    "plt.plot([oy, np.zeros(np.size(oy))], [ox, np.zeros(np.size(oy))], \"ro-\") # lines from 0,0 to the \n",
    "plt.axis(\"equal\")\n",
    "bottom, top = plt.ylim()  # return the current ylim\n",
    "plt.ylim((top, bottom)) # rescale y axis, to match the grid orientation\n",
    "plt.grid(True)\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The `lidar_to_grid_map.py` contains handy functions which can used to convert a 2D range measurement to a grid map. For example the `bresenham`  gives the a straight line between two points in a grid map. Let's see how this works."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 6700 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 2 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "import lidar_to_grid_map as lg\n",
    "map1 = np.ones((50, 50)) * 0.5\n",
    "line = lg.bresenham((2, 2), (40, 30))\n",
    "for l in line:\n",
    "    map1[l[0]][l[1]] = 1\n",
    "plt.imshow(map1)\n",
    "plt.colorbar()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 6716 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 2 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "line = lg.bresenham((2, 30), (40, 30))\n",
    "for l in line:\n",
    "    map1[l[0]][l[1]] = 1\n",
    "line = lg.bresenham((2, 30), (2, 2))\n",
    "for l in line:\n",
    "    map1[l[0]][l[1]] = 1\n",
    "plt.imshow(map1)\n",
    "plt.colorbar()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "To fill empty areas, a queue-based algorithm can be used that can be used on an initialized occupancy map. The center point is given: the algorithm checks for neighbour elements in each iteration, and stops expansion on obstacles and free boundaries."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "from collections import deque\n",
    "def flood_fill(cpoint, pmap):\n",
    "    \"\"\"\n",
    "    cpoint: starting point (x,y) of fill\n",
    "    pmap: occupancy map generated from Bresenham ray-tracing\n",
    "    \"\"\"\n",
    "    # Fill empty areas with queue method\n",
    "    sx, sy = pmap.shape\n",
    "    fringe = deque()\n",
    "    fringe.appendleft(cpoint)\n",
    "    while fringe:\n",
    "        n = fringe.pop()\n",
    "        nx, ny = n\n",
    "        # West\n",
    "        if nx > 0:\n",
    "            if pmap[nx - 1, ny] == 0.5:\n",
    "                pmap[nx - 1, ny] = 0.0\n",
    "                fringe.appendleft((nx - 1, ny))\n",
    "        # East\n",
    "        if nx < sx - 1:\n",
    "            if pmap[nx + 1, ny] == 0.5:\n",
    "                pmap[nx + 1, ny] = 0.0\n",
    "                fringe.appendleft((nx + 1, ny))\n",
    "        # North\n",
    "        if ny > 0:\n",
    "            if pmap[nx, ny - 1] == 0.5:\n",
    "                pmap[nx, ny - 1] = 0.0\n",
    "                fringe.appendleft((nx, ny - 1))\n",
    "        # South\n",
    "        if ny < sy - 1:\n",
    "            if pmap[nx, ny + 1] == 0.5:\n",
    "                pmap[nx, ny + 1] = 0.0\n",
    "                fringe.appendleft((nx, ny + 1))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "This algotihm will fill the area bounded by the yellow lines starting from a center point (e.g. (10, 20)) with zeros:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 6740 characters]\n",
      "text/plain": [
       "<Figure size 432x288 with 2 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "flood_fill((10, 20), map1)\n",
    "map_float = np.array(map1)/10.0\n",
    "plt.imshow(map1)\n",
    "plt.colorbar()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Let's use this flood fill on real data:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The grid map is  150 x 100 .\n"
     ]
    },
    {
     "data": {
      "image/png": "[encoded data, 18776 characters]\n",
      "text/plain": [
       "<Figure size 1440x576 with 2 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "xyreso = 0.02  # x-y grid resolution\n",
    "yawreso = math.radians(3.1)  # yaw angle resolution [rad]\n",
    "ang, dist = file_read(\"lidar01.csv\")\n",
    "ox = np.sin(ang) * dist\n",
    "oy = np.cos(ang) * dist\n",
    "pmap, minx, maxx, miny, maxy, xyreso = lg.generate_ray_casting_grid_map(ox, oy, xyreso, False)\n",
    "xyres = np.array(pmap).shape\n",
    "plt.figure(figsize=(20,8))\n",
    "plt.subplot(122)\n",
    "plt.imshow(pmap, cmap = \"PiYG_r\") \n",
    "plt.clim(-0.4, 1.4)\n",
    "plt.gca().set_xticks(np.arange(-.5, xyres[1], 1), minor = True)\n",
    "plt.gca().set_yticks(np.arange(-.5, xyres[0], 1), minor = True)\n",
    "plt.grid(True, which=\"minor\", color=\"w\", linewidth = .6, alpha = 0.5)\n",
    "plt.colorbar()\n",
    "plt.show()"
   ]
  }
 ],
 "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.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
