slides.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "skip"
    }
   },
   "outputs": [],
   "source": [
    "# Note: Before starting the slideshow, make sure you run this code as it\n",
    "# provides helper functions that the other slides need...\n",
    "#\n",
    "# Other than that, you can ignore this content as it won't show up in the slideshow.\n",
    "#\n",
    "# It's a helper function that makes it easier to show OpenCV images directly\n",
    "# in the notebook environment. When using OpenCV locally, you'll want to use\n",
    "# `cv2.imshow(name, img)` instead.\n",
    "\n",
    "from scipy.spatial import distance as dist\n",
    "import numpy as np\n",
    "import cv2\n",
    "import math\n",
    "\n",
    "# Notebook setup + convenience functions\n",
    "%matplotlib inline\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "def force_bgr(img):\n",
    "    '''Forces image to 3-channel representation if grayscale'''\n",
    "    if len(img.shape) == 2 or img.shape[2] == 1:\n",
    "        return cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)\n",
    "    return img\n",
    "\n",
    "def imshow(*args):\n",
    "    '''Helper function to show images, because matplotlib and OpenCV aren't a perfect match'''\n",
    "    fig = plt.figure()\n",
    "    for i, img in enumerate(args):\n",
    "        fig.add_subplot(1,len(args),i+1)\n",
    "        plt.imshow(cv2.cvtColor(force_bgr(img), cv2.COLOR_BGR2RGB))\n",
    "\n",
    "def blur(src, radius):\n",
    "    ksize = int(2 * round(radius) + 1)\n",
    "    return cv2.blur(src, (ksize, ksize))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "\n",
    "# <center>Image processing using</center>\n",
    "\n",
    "## <center>OpenCV + Python</center>\n",
    "\n",
    "<br/>\n",
    "<center><b>Tim Winters</b><center>\n",
    "<center>Created by Dustin Spicuzza (Team 2423/1418)</center>\n",
    "<br/>\n",
    "<center>September 10, 2016</center>\n",
    "<br/>\n",
    "<center><span style=\"color: #ababab\">NE FIRST University Day</span></center>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Agenda\n",
    "\n",
    "* Why OpenCV + Python?\n",
    "* Image filtering demo\n",
    "* pynetworktables"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Image processing\n",
    "\n",
    "* FRC Teams do it a lot of ways\n",
    "    * NIVision (LabVIEW)\n",
    "    * GRIP (Uses OpenCV as engine)\n",
    "    * OpenCV (various custom stuff)\n",
    "    \n",
    "* We're going to talk about OpenCV"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Why OpenCV?\n",
    "\n",
    "* Originally developed by Intel\n",
    "* It has thousands of image processing related algorithms and functions available\n",
    "  * Highly optimized and reliable\n",
    "  * Has building blocks that fit together\n",
    "* Lets you do complex image processing without needing to understand the math\n",
    "  * If you understand the math, it helps!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Why OpenCV?\n",
    "\n",
    "* Bindings for multiple languages\n",
    "    * C/C++\n",
    "    * Java\n",
    "    * Python\n",
    "* Multiple platforms supported\n",
    "    * Windows\n",
    "    * Linux\n",
    "    * OSX\n",
    "    * Android\n",
    "* Oh, and it’s **FREE**!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# What OpenCV Provides\n",
    "\n",
    "* Image I/O:\n",
    "    * Read/Write images from disk\n",
    "    * Use native OS functionality to interface with cameras\n",
    "* Image Segmentation\n",
    "    * Edge finding\n",
    "    * Contour detection\n",
    "    * Thresholding"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# What OpenCV Provides\n",
    "\n",
    "* Face detection\n",
    "* Motion tracking\n",
    "* Stereo vision support\n",
    "* Support for GPU acceleration\n",
    "* Machine learning operations\n",
    "    * Classifiers\n",
    "    * Neural networks"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# What OpenCV Provides\n",
    "\n",
    "* Distributed with lots of useful samples that you can use to figure out how OpenCV works\n",
    "    * Face detection\n",
    "    * Edge finding\n",
    "    * Histograms\n",
    "    * Square finder\n",
    "\n",
    "Lots and lots and lots of stuff… "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Why Python + OpenCV?\n",
    "\n",
    "* Python is really easy to learn and use\n",
    "    * Simple syntax\n",
    "    * Rapid prototyping\n",
    "* Most of the compute intensive work is implemented in C/C++\n",
    "    * Python is just glue, realtime operation **is** possible\n",
    "* NumPy is awesome\n",
    "    * Manipulating image data is trivial compared to other OpenCV bindings (Java, C++)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# <center>Time to CODE!</center>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "source": [
    "<center>Go to http://goo.gl/nB0NCG</center>"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# About this environment\n",
    "\n",
    "http://goo.gl/nB0NCG\n",
    "\n",
    "* It's a Jupyter Notebook (formerly IPython Notebook)\n",
    "    * This slideshow uses Jupyter too!\n",
    "    \n",
    "* It allows you to mix text and executable code in a webpage\n",
    "* You execute each cell using SHIFT-ENTER"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Hello World!\n",
    "\n",
    "* Click the cell with the following text, and press SHIFT-ENTER"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "print(\"Hello class\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Next Steps\n",
    "\n",
    "* Execute the helper code"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "source": [
    "* The next cell tells you about the images available in your environment"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%ls images"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Hello image!\n",
    "\n",
    "* Let's load an image and show it"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "-"
    }
   },
   "outputs": [],
   "source": [
    "# Change this to load different images\n",
    "img = cv2.imread('images/2016-cmp-5.jpg')\n",
    "imshow(img)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Hello image!\n",
    "\n",
    "* You can show multiple images next to each other"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "imshow(img, img)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# OpenCV Image Basics\n",
    "\n",
    "* Images are stored as multidimensional arrays\n",
    "    * Color images have 3 dimensions: height, width, channel\n",
    "* Each pixel is a number stored in the array\n",
    "* Numpy array notation allows you to do operations on individual pixels or ranges of pixels"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "img[50, 150, :]           # Access a single pixel,"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x = img[24:42, 42:100, :]    # Access a range of pixels"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# OpenCV Image Basics\n",
    "\n",
    "* Color is represented by storing combinations of Red, Blue, and Green pixels in separate channels\n",
    "    * OpenCV uses BGR representation, not RGB\n",
    "* The amount of each individual color is represented in the individual channel\n",
    "    * ‘dark’ is zero, ‘bright’ is 255\n",
    "* Combine the channels to represent a color\n",
    "    * Green = RGB( 0, 255, 0 )\n",
    "    * Deep Pink = RGB( 255, 20, 147 )"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# OpenCV Image Basics\n",
    "\n",
    "* Using numpy we can easily fill an image with a single color"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "-"
    }
   },
   "outputs": [],
   "source": [
    "# define image with height=240, width=320, 3 channels\n",
    "shape = (240, 320, 3)\n",
    "pink_img = np.empty(shape, dtype=np.uint8)\n",
    "\n",
    "# Fill every pixel with a single color\n",
    "pink_img[:] = (147, 20, 255)\n",
    "\n",
    "imshow(pink_img)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Practical Example\n",
    "\n",
    "* 2016 FIRST Stronghold: find targets that are surrounded by retroreflective tape, and shoot boulders into them\n",
    "* 2017 FIRST Steamworks: use two targets to align to gear holder on the ship"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Practical Example\n",
    "* Finding gray tape at a distance isn’t particularly easy\n",
    "    * Key part of image processing is removing as much non-essential information from image \n",
    "* We can do better!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Retroreflective Tape\n",
    "\n",
    "* It has a useful property -- it reflects light directly back at the source\n",
    "* What can we do with this property?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "fragment"
    }
   },
   "source": [
    "* Shine bright LEDs at the target and the tape reflects that color back to the camera\n",
    "* Reduce exposure of camera so only bright light sources are seen "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "# What Color Light?\n",
    "\n",
    "* Humans are most sensitive to green light, and cameras mimic this\n",
    " * Twice as many green sensors as red or blue\n",
    "\n",
    "This makes green light best suited for vision"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# A note about exposure\n",
    "\n",
    "* Webcams support setting the exposure manually (yay)\n",
    "* Some cameras only allow particular exposure settings \n",
    "    * The lifecam is one of them\n",
    "* OpenCV has bugs, it doesn't set the exposure properly\n",
    "* Here's a workaround that works on linux:\n",
    "\n",
    "```sh \n",
    "v4l2-ctl -d /dev/video0 -c exposure_auto=1 -c exposure_absolute=10\n",
    "```"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Retroreflective Tape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "img1 = cv2.imread('images/2016-p0.jpg')\n",
    "img2 = cv2.imread('images/2016-p1.jpg')\n",
    "imshow(img1)\n",
    "imshow(img2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Practical Example\n",
    "\n",
    "Processing steps to find targets:\n",
    "\n",
    "* Isolate the green portions of the image\n",
    "* Analyze the green portions to determine targets\n",
    "\n",
    "**Note**: There are a lot of ways to go about this, I'm just showing you one way"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Blurring the Image\n",
    "\n",
    "  * Get rid of small artifacts (noise)\n",
    "  * Makes the target a little more complete\n",
    "    * Easier for thresholding and contour finding"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "blurred = blur(img2, 2)\n",
    "imshow(blurred)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Identify the green\n",
    "\n",
    "* What is “green” anyways?\n",
    "    * <span style=\"color: green\">This is green.</span> <span style=\"color: darkgreen\">This is also green.</span>\n",
    "* To a computer, green is really a range of colors\n",
    "* An object’s color changes depending on lighting conditions\n",
    "* We can transform the image to identify colors independent of lighting conditions"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Identify the green\n",
    "\n",
    "* Convert the image from RGB to HSV\n",
    "    * Hue: the color\n",
    "    * Saturation: Colorfulness\n",
    "    * Value: Brightness"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "-"
    }
   },
   "outputs": [],
   "source": [
    "hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)\n",
    "imshow(img2)\n",
    "imshow(hsv)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Identify the green\n",
    "\n",
    "That doesn't show why HSV is useful. Let's look at the individual channels instead."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "-"
    }
   },
   "outputs": [],
   "source": [
    "h, s, v = cv2.split(hsv)\n",
    "imshow(h)\n",
    "imshow(s)\n",
    "imshow(v)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Identify the green\n",
    "\n",
    "* Green is a range of values present in the image\n",
    "* ‘Threshold’ the image to get rid of the colors that we don’t care about\n",
    "* Lots of ways to do this\n",
    "    * Manually specify values\n",
    "    * Automated methods"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Identify the green\n",
    "\n",
    "`cv2.inRange` can threshold an image given two ranges of pixels.\n",
    "* Wanted values are converted to 255\n",
    "* Unwanted values are now 0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "-"
    }
   },
   "outputs": [],
   "source": [
    "lower = np.array([0, 145, 80])\n",
    "upper = np.array([255, 255, 255])\n",
    "\n",
    "filtered = cv2.inRange(hsv, lower, upper)\n",
    "imshow(img2)\n",
    "imshow(filtered)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Identify the green\n",
    "\n",
    "Sometimes, you end up with holes in your output"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "img3 = cv2.imread('images/2013-f0.png')\n",
    "hsv3 = cv2.cvtColor(img3, cv2.COLOR_BGR2HSV)\n",
    "blur3 = hsv3\n",
    "blur3 = blur(hsv3, 2)\n",
    "\n",
    "# Thresholds are different because different camera/lighting\n",
    "lower3 = np.array([30, 175, 16])\n",
    "upper3 = np.array([75, 255, 255])\n",
    "filtered3 = cv2.inRange(blur3, lower3, upper3)\n",
    "imshow(img3)\n",
    "imshow(filtered3)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Filling in the holes\n",
    "\n",
    "* We can use a morphological operation to fill in the holes\n",
    "    * Various types of morphology operations available\n",
    "* They modify a pixel based on the values of its neighboring pixels\n",
    "    * The one we use to fill in holes is a “closing” operation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "outputs": [],
   "source": [
    "# kernel tells opencv how big a space to sample, and what shape to sample.\n",
    "kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (2,2), anchor=(1,1))\n",
    "# sample and close 3 times\n",
    "output = cv2.morphologyEx(filtered3, cv2.MORPH_CLOSE, kernel,\n",
    "                          iterations=3)\n",
    "imshow(output)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "collapsed": true,
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Identifying Targets\n",
    "\n",
    "Use `findContours()` to find regions of interest\n",
    "* Returns a list of points bounding each separate blob in the image (called a contour)\n",
    "* Also returns a hierarchy so you can determine whether a contour is entirely inside another contour"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "-"
    }
   },
   "outputs": [],
   "source": [
    "contours, _ = cv2.findContours(output, cv2.RETR_EXTERNAL,\n",
    "                                    cv2.CHAIN_APPROX_SIMPLE)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Identifying Targets\n",
    "\n",
    "If you want to see what it found, you can draw the found contours."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "-"
    }
   },
   "outputs": [],
   "source": [
    "dst = np.zeros(shape=img3.shape, dtype=img3.dtype)\n",
    "cv2.drawContours(dst, contours, -1, (0, 255, 255), 3)\n",
    "imshow(dst)\n",
    "#print(contours[0])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Identifying Targets\n",
    "\n",
    "* As you can see, contours aren't the whole story\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Identifying Targets\n",
    "\n",
    "* Contour analysis\n",
    "    * Discard non-convex contours\n",
    "    * Convert to polygon approximation (approxPolyDP)\n",
    "        * This will turn our multi-vertex contour into a contour with the vertex we see in the real world\n",
    "    * Discard polygons that aren't the right size\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Magic?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "-"
    }
   },
   "outputs": [],
   "source": [
    "min_width = 20  # in pixels \n",
    "min_height = 50\n",
    "results = []\n",
    "centers = []\n",
    "\n",
    "# Iterate over each contour\n",
    "for cnt in contours:\n",
    "    \n",
    "    # Skip initial contours that don't meet requirements\n",
    "    _, _, w, h = cv2.boundingRect(cnt)\n",
    "    if w < h or w < min_width: # or len(a2) not in (4, 5):\n",
    "        continue\n",
    "    \n",
    "    # Find approximation that contains 4 sides\n",
    "    points_wanted = 4\n",
    "    precision = 100\n",
    "    approx = None\n",
    "    for x in range(precision):\n",
    "        epsilon = (x/precision)*cv2.arcLength(cnt, True)\n",
    "        approx = cv2.approxPolyDP(cnt, epsilon, True)\n",
    "        if approx.shape[0] == points_wanted:\n",
    "            break\n",
    "\n",
    "    # We only care about objects that are wider than they are tall, and things wider\n",
    "    # than a particular width. Only keep things that meet that criteria.\n",
    "    _, _, w, h = cv2.boundingRect(approx)\n",
    "    if w < h or w < min_width or h < min_height: # or len(a2) not in (4, 5):\n",
    "        continue\n",
    "    \n",
    "    results.append(approx.squeeze(1))\n",
    "    M = cv2.moments(approx)\n",
    "    if M[\"m00\"] == 0:\n",
    "        continue\n",
    "    cX = int(M[\"m10\"] / M[\"m00\"])\n",
    "    cY = int(M[\"m01\"] / M[\"m00\"])\n",
    "\n",
    "    # draw the contour and center of the shape on the image\n",
    "    centers.append((int(cX), int(cY)))\n",
    "    cv2.putText(dst, \"center\", (cX - 20, cY - 20),\n",
    "                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)\n",
    "    \n",
    "dst2 = np.zeros(shape=img3.shape, dtype=img3.dtype)\n",
    "cv2.drawContours(dst2, results, -1, (0, 255, 255), 3)\n",
    "imshow(dst)\n",
    "imshow(dst2)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Contour Features\n",
    "You can find more about contour features and properties [here](https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_imgproc/py_contours/py_table_of_contents_contours/py_table_of_contents_contours.html)\n",
    "\n",
    "For 2020, we will want to heavily use the convex hull and "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Magic?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "slideshow": {
     "slide_type": "-"
    }
   },
   "outputs": [],
   "source": [
    "# Finally, draw out our results\n",
    "for cnt in centers:\n",
    "    cv2.circle(dst2, (cnt[0], cnt[1]), 1, (255, 255, 255), 5)\n",
    "imshow(dst2)\n",
    "#print(results[0])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Identifying Targets\n",
    "\n",
    "* Sometimes you need to do more work\n",
    "    * Use ratios to determine which target you're looking at\n",
    "    * Remove duplicates (inner rectangles)\n",
    "    * Other types of validation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Now what?\n",
    "\n",
    "We have targets... probably should do something with them?"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Lets sort the targets left to right, using the x value of the center\n",
    "def getXValueOfCenter(cnta):\n",
    "    M = cv2.moments(cnta)\n",
    "    if M[\"m00\"] == 0:\n",
    "        return 0\n",
    "    return int(M[\"m10\"] / M[\"m00\"])\n",
    "\n",
    "results = sorted(results, key=lambda c: getXValueOfCenter(c))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "# Image Points\n",
    "\n",
    "This is the hardest part of the whole process. Singling out the chosen obejct points, and making sure they're in the right order.\n",
    "OpenCV doesn't have a strict method of ordering contours, so we have to do some manual finding of the contours.\n",
    "Luckily, we're dealing with squares so it's easy to figure this out."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Now lets sort the coordinates for each contour clockwise starting with top left\n",
    "\n",
    "# taken from https://www.pyimagesearch.com/2016/03/21/ordering-coordinates-clockwise-with-python-and-opencv/\n",
    "def order_points(pts):\n",
    "    # sort the points based on their x-coordinates\n",
    "    xSorted = pts[np.argsort(pts[:, 0]), :]\n",
    "    # grab the left-most and right-most points from the sorted\n",
    "    # x-roodinate points\n",
    "    leftMost = xSorted[:2, :]\n",
    "    rightMost = xSorted[2:, :]\n",
    "    # now, sort the left-most coordinates according to their\n",
    "    # y-coordinates so we can grab the top-left and bottom-left\n",
    "    # points, respectively\n",
    "    leftMost = leftMost[np.argsort(leftMost[:, 1]), :]\n",
    "    (tl, bl) = leftMost\n",
    "    # now that we have the top-left coordinate, use it as an\n",
    "    # anchor to calculate the Euclidean distance between the\n",
    "    # top-left and right-most points; by the Pythagorean\n",
    "    # theorem, the point with the largest distance will be\n",
    "    # our bottom-right point\n",
    "    D = dist.cdist(tl[np.newaxis], rightMost, \"euclidean\")[0]\n",
    "    (br, tr) = rightMost[np.argsort(D)[::-1], :]\n",
    "    # return the coordinates in top-left, top-right,\n",
    "    # bottom-right, and bottom-left order\n",
    "    return np.array([tl, tr, br, bl], dtype=\"int32\")\n",
    "\n",
    "results = np.array(list(map(order_points, results)))\n",
    "print(results)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Calculate angle/distance to target\n",
    "\n",
    "* If we know where the object is in the real world, we can map out the camera position using the object's position in the image\n",
    "* Slight adjustments for inaccuracies in the camera are needed\n",
    "\n",
    "* We need a couple parts for this\n",
    "    * Camera Calibration Matrix\n",
    "    * Camera Distortion Coefficients\n",
    "    * Object Points\n",
    "    * Image Points\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "# Camera Calibration Matrix\n",
    "OpenCV has functions built in to get your camera's calibration numbers\n",
    "\n",
    "Luckily, you don't actually have to understand the code to get your calibration, but you can find the code [here](https://github.com/opencv/opencv/blob/master/samples/python/calibrate.py)\n",
    "\n",
    "To begin, we need ~25 images taken with the camera of a checkerboard pattern. Laser cutting onto some 1/4\" project panels works well, since this will give you a nice rigid surface\n",
    "\n",
    "To generate the matrix from the raspberry pi, use the `cameraCapture.py` file in the repo."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "# Camera Distortion Coefficients\n",
    "\n",
    "If you've ever seen a camera that has a fisheye lens, then you know what lens distortion looks like.\n",
    "This makes it hard to get good distance calculations, so we need to know how distorted the image is.\n",
    "Luckily, the camera calibration gives us both the intrinsic properties of the camera, as well as the \n",
    "distortion coefficients, so we're done!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "# Object Points\n",
    "\n",
    "Perspective-n-Point works by mapping the 2d pixel coordinates to 3d coordinates in the real world.\n",
    "In order to map these, we need to define which points we're going to reference. For 2020, the target is a half hexagon\n",
    "We can use the four outer vertices as our points, with the center of the hexagon as our 'origin'.\n",
    "To calculate the 3d coordinates for the 4 points, it is helpful to create a CAD drawing and use the measurement tools.\n",
    "\n",
    "Take some time now and decide what points you'd like to use for vision targeting. You need at least 4, and the points should\n",
    "  easy to distinguish."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "subslide"
    }
   },
   "source": [
    "# Object Points for 2013 Targets\n",
    "\n",
    "We can look at the field diagram to generate the object points. I modelled it in fusion360 and came up with the following. Origin is at the center of the high goal.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Store object points\n",
    "\n",
    "obj_points = np.array([(-103, 3.5, 0),\n",
    "                        (-41, 3.5, 0),\n",
    "                        (-41, -25.50, 0),\n",
    "                        (-103, -25.50, 0),\n",
    "                        (-31, 10, 0),\n",
    "                        ( 31, 10, 0),\n",
    "                        (31, -10, 0),\n",
    "                        (-31, -10, 0),\n",
    "                        (41, 3.5, 0),\n",
    "                        (103, 3.5, 0),\n",
    "                        (103, -25.50, 0),\n",
    "                        (41, -25.50, 0)], dtype=np.float32)\n",
    "\n",
    "# made up matrix using 4.4mm focal length found online\n",
    "cam_matrix = np.array([[.17, 0, 320],\n",
    "                       [0, .17, 240],\n",
    "                       [0,  0,  1]], dtype=np.float)\n",
    "\n",
    "# pretending no distortion\n",
    "dist_coeffs = np.array([1.3287735059062630e-01, -6.8376776294978103e-01, 0., 0., 8.1215478360827675e-01])\n",
    "# flatten list of points\n",
    "results = results.reshape(12, 2).astype('float32')\n",
    "axis = np.float32([[24,0,0], [0,24,0], [0,0,-24]]).reshape(-1,3)\n",
    "print(results)\n",
    "print(obj_points)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Solve PnP\n",
    "The last part of the problem is solving the PnP equation by calling a single method\n",
    "\n",
    "```\n",
    "ret, rvecs, tvecs = cv2.solvePnP(object_points, image_points, cam_matrix, dist_coeffs)\n",
    "```\n",
    "\n",
    "`rvecs` will be a vector of the rotations in radians\n",
    "`tvecs` will be a vector of the translations in whatever unit you specify for the object points. This tells us the \n",
    "  distance to the target"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# THIS IS THE GOD FUNCTION\n",
    "ret, rvecs, tvecs = cv2.solvePnP(obj_points, results, cam_matrix, dist_coeffs)\n",
    "\n",
    "print(\"Translation Vectors\")\n",
    "print(tvecs)\n",
    "# function used to draw \n",
    "def draw(img, imgpts):\n",
    "    corner = centers[2]\n",
    "    img = cv2.line(img, corner, tuple(imgpts[0].ravel()), (255, 0, 0), 5)\n",
    "    img = cv2.line(img, corner, tuple(imgpts[1].ravel()), (0, 255, 0), 5)\n",
    "    img = cv2.line(img, corner, tuple(imgpts[2].ravel()), (0, 0, 255), 5)\n",
    "    return img\n",
    "\n",
    "\n",
    "# this probably doesn't work because my camera numbers are off. But it would IRL!\n",
    "imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, cam_matrix, dist_coeffs)\n",
    "img = draw(dst2, imgpts.astype(np.int))\n",
    "imshow(img)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Now What?\n",
    "\n",
    "* Send data via NetworkTables\n",
    "\n",
    "* ... I forgot to write this slide. It's easy, I promise."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Where to run the image processing\n",
    "\n",
    "* RoboRIO\n",
    "    * RoboRIO is relatively slow, OpenCV eats a lot of CPU\n",
    "        * Hint: Make the images small (320x240)\n",
    "    * Less hardware to deal with\n",
    "    * FIRST intends to install OpenCV by default in 2017\n",
    "    "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Where to run the image processing\n",
    "\n",
    "* Driver Station\n",
    "    * Streaming images to OpenCV is possible\n",
    "        * Various latency bugs\n",
    "    * Latency is an issue here\n",
    "    * mDNS problems (hopefully will be resolved in 2017)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Where to run the image processing\n",
    "\n",
    "* Coprocessor (Jetson, Raspberry PI, Nexus 5)\n",
    "    * Lots of teams do this\n",
    "    * More hardware to deal with\n",
    "    * Potentially higher fidelity processing"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Want code?\n",
    "\n",
    "* Working OpenCV code integrated with mjpg-streamer\n",
    "    * https://github.com/frc2423/2016/tree/master/OpenCV\n",
    "    * Includes code for storing images onto USB drive during matches\n",
    "    * Don't let our robot's performance fool you... :(\n",
    "    \n",
    "* The stuff we did here will be available sometime tonight\n",
    "    * https://github.com/virtuald/frc-imageprocessing-workshop-2016"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# If you want more\n",
    "\n",
    "* Team 254 gave an excellent presentation at CMP in 2016\n",
    "    * https://goo.gl/mppi4E\n",
    "    * Video/audio: http://www.chiefdelphi.com/forums/showthread.php?t=147568&page=3\n",
    "    * Latency compensation is an excellent technique presented here"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Resources\n",
    "\n",
    "* Python 3.5.x\n",
    "    * https://www.python.org/downloads/\n",
    "* Learn Python\n",
    "    * http://www.codecademy.com/tracks/python\n",
    "\n",
    "* OpenCV 3.1.0\n",
    "    * http://opencv.org\n",
    "\n",
    "* NumPy\n",
    "    * Official site: http://www.numpy.org"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Resources\n",
    "\n",
    "* roborio-packages\n",
    "    * https://github.com/robotpy/roborio-packages\n",
    "* OpenCV for RoboRIO\n",
    "    * https://github.com/robotpy/roborio-opencv\n",
    "* mjpg-streamer for RoboRIO\n",
    "    * https://github.com/robotpy/mjpg-streamer"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# Resources\n",
    "\n",
    "* pynetworktables\n",
    "    * source code + examples @\n",
    "      https://github.com/robotpy/pynetworktables\n",
    "\n",
    "* Edit & debug python code using Eclipse\n",
    "    * Pydev: http://pydev.org/"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# One more thing...\n",
    "\n",
    "FIRSTwiki: https://firstwiki.github.io\n",
    "\n",
    "* Publicly editable repository of information related to FIRST Robotics\n",
    "    * Technical topics\n",
    "    * Non-technical\n",
    "    * Team pages\n",
    "* Add content to your team’s page!"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "slideshow": {
     "slide_type": "slide"
    }
   },
   "source": [
    "# <center>Questions?</center>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "celltoolbar": "Slideshow",
  "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.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}