SLAM A 08.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 72,
   "metadata": {},
   "outputs": [],
   "source": [
    "from pylab import *\n",
    "from lego_robot import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 73,
   "metadata": {},
   "outputs": [],
   "source": [
    "def compute_derivative(scan, min_dist):\n",
    "    jumps = [ 0 ]\n",
    "    for i in range(1, len(scan) - 1):\n",
    "        # --->>> Insert your code here.\n",
    "        # Compute derivative using formula \"(f(i+1) - f(i-1)) / 2\".\n",
    "        l = scan[i-1]\n",
    "        r = scan[i+1]\n",
    "        \n",
    "        # Do not use erroneous scan values, which are below min_dist.\n",
    "        if (l > min_dist and r > min_dist): \n",
    "            der = (r-l)/2.0\n",
    "            jumps.append(der) # Replace this line, append derivative instead.\n",
    "        else: \n",
    "            jumps.append(0)\n",
    "    return jumps"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 74,
   "metadata": {},
   "outputs": [],
   "source": [
    "def find_cylinders(scan, scan_derivative, jump, min_dist):\n",
    "    cylinder_list = []\n",
    "    on_cylinder = False\n",
    "    sum_rays, sum_depths, rays = 0.0, 0.0, 0\n",
    "\n",
    "    for i in range(len(scan_derivative)):\n",
    "        # --->>> Insert your cylinder code here.\n",
    "        # Whenever you find a cylinder, add a tuple\n",
    "        # (average_ray, average_depth) to the cylinder_list.\n",
    "        if (abs(scan_derivative[i]) > jump): #trigger condition\n",
    "            if (scan_derivative[i] < 0): #left edge\n",
    "                if (on_cylinder == False): #on cylinder\n",
    "                    on_cylinder = True\n",
    "                else: #LL\n",
    "                    sum_rays = 0.0\n",
    "                    sum_depths = 0.0\n",
    "                    rays = 0\n",
    "                        \n",
    "            else: #right edge\n",
    "                if (on_cylinder == True): #ignore RR\n",
    "                    on_cylinder = False\n",
    "                    cylinder_list.append((sum_rays/rays,sum_depths/rays))\n",
    "                    sum_rays = 0.0\n",
    "                    sum_depths = 0.0\n",
    "                    rays = 0\n",
    "                    \n",
    "        if (on_cylinder == True):\n",
    "            if (scan[i]>min_dist):\n",
    "                rays += 1\n",
    "                sum_rays += i\n",
    "                sum_depths += scan[i]\n",
    "\n",
    "    return cylinder_list"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 75,
   "metadata": {},
   "outputs": [],
   "source": [
    "def compute_cartesian_coordinates(cylinders, cylinder_offset):\n",
    "    result = []\n",
    "    for (bindex,rangec) in cylinders:\n",
    "        # --->>> Insert here the conversion from polar to Cartesian coordinates.\n",
    "        # c is a tuple (beam_index, range).\n",
    "        # For converting the beam index to an angle, use\n",
    "        # LegoLogfile.beam_index_to_angle(beam_index)\n",
    "        index_as_radians = LegoLogfile.beam_index_to_angle(bindex)\n",
    "        x = cos(index_as_radians) * (rangec + cylinder_offset)\n",
    "        y = sin(index_as_radians) * (rangec + cylinder_offset)\n",
    "        result.append( (x,y) ) # Replace this by your (x,y)\n",
    "    return result"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 76,
   "metadata": {},
   "outputs": [],
   "source": [
    "logfile = LegoLogfile()\n",
    "logfile.read(\"robot4_scan.txt\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 77,
   "metadata": {},
   "outputs": [],
   "source": [
    "minimum_valid_distance = 25.0\n",
    "depth_jump = 100.0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 78,
   "metadata": {},
   "outputs": [],
   "source": [
    "scan_no = 7\n",
    "scan = logfile.scan_data[scan_no]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 79,
   "metadata": {},
   "outputs": [],
   "source": [
    "der = compute_derivative(scan, minimum_valid_distance)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 80,
   "metadata": {},
   "outputs": [],
   "source": [
    "cylinders = find_cylinders(scan, der, depth_jump, minimum_valid_distance)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 81,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[(232.0, 374.93548387096774),\n",
       " (289.5, 1403.2),\n",
       " (364.0, 1668.111111111111),\n",
       " (416.5, 1176.75),\n",
       " (476.5, 713.3),\n",
       " (499.5, 1505.5)]"
      ]
     },
     "execution_count": 81,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "cylinders"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 82,
   "metadata": {},
   "outputs": [],
   "source": [
    "    minimum_valid_distance = 20.0\n",
    "    depth_jump = 100.0\n",
    "    cylinder_offset = 90.0\n",
    "\n",
    "    # Read the logfile which contains all scans.\n",
    "    logfile = LegoLogfile()\n",
    "    logfile.read(\"robot4_scan.txt\")\n",
    "\n",
    "    # Write a result file containing all cylinder records.\n",
    "    # Format is: D C x[in mm] y[in mm] ...\n",
    "    # With zero or more points.\n",
    "    # Note \"D C\" is also written for otherwise empty lines (no\n",
    "    # cylinders in scan)\n",
    "    out_file = open(\"cylinders.txt\", \"w\")\n",
    "    for scan in logfile.scan_data:\n",
    "        # Find cylinders.\n",
    "        der = compute_derivative(scan, minimum_valid_distance)\n",
    "        cylinders = find_cylinders(scan, der, depth_jump,\n",
    "                                   minimum_valid_distance)\n",
    "        cartesian_cylinders = compute_cartesian_coordinates(cylinders,\n",
    "                                                            cylinder_offset)\n",
    "        # Write to file.\n",
    "       \n",
    "        out_file.write(\"D C \")\n",
    "        for c in cartesian_cylinders:\n",
    "            out_file.write(\"%.1f %.1f \" % c)\n",
    "        out_file.write(\"\\n\")\n",
    "    out_file.close()\n"
   ]
  },
  {
   "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.5.3"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}