{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Lane Boundary Projection"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Setting up Colab"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "colab_nb = 'google.colab' in str(get_ipython())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "if colab_nb:\n",
    "  from google.colab import drive\n",
    "  drive.mount('/content/drive')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "if colab_nb:\n",
    "  %cd /content/drive/My Drive/aad/code/tests/lane_detection"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Introduction"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this exercise we will apply or knowledge of image formation to create label data for our deep learning model. To train that model we need lots of (input, output) examples. The inputs are images from a camera behind the wind shield of our vehicle. For the expected model output, we need to label each pixel in an image as being part of the left boundary, part of the right boundary, or neither of those."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Getting images is easy with Carla: We can attach a camera to a vehicle and store the image that this camera takes. If you want to see details, you can check out `collect_data.py`. Let's have a look at an exemplary image:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "from pathlib import Path\n",
    "import cv2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "image_fn = str(Path(\"../../../data/Town04_Clear_Noon_09_09_2020_14_57_22_frame_625_validation_set.png\"))\n",
    "image = cv2.imread(image_fn)\n",
    "image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)\n",
    "plt.imshow(image)\n",
    "image.shape"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Each time the `collect_data.py` captures an image, it also writes down **[polylines](https://en.wikipedia.org/wiki/Polygonal_chain) in world coordinates** that represent the left and right lane boundary respectively. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "boundary_fn = image_fn.replace(\".png\", \"_boundary.txt\")\n",
    "boundary_gt = np.loadtxt(boundary_fn)\n",
    "# exploit that in the Carla world coordinates the road is mostly in the Xw-Yw plane\n",
    "print(\"Zw-coords: \", boundary_gt[:,2])\n",
    "plt.plot(boundary_gt[:,0], boundary_gt[:,1], label=\"left lane boundary\")\n",
    "plt.plot(boundary_gt[:,3], boundary_gt[:,4], label=\"right lane boundary\")\n",
    "plt.axis(\"equal\")\n",
    "plt.legend();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now to get the label image, we need to take the world coordinates of the lane boundaries, transform them into the camera coordinate system, and then project them to image coordinates $(u,v)$ using the intrinsic matrix $K$.\n",
    "The transformation from the world coordinate frame to the camera centered camera frame depends on the pose of the vehicle, to which the camera is attached. Carla makes it easy to obtain such transformation matrices, and we actually stored the transformation matrix corresponding to the image we are just looking at. Let's load it:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "trafo_fn = image_fn.replace(\".png\", \"_trafo.txt\")\n",
    "trafo_world_to_cam = np.loadtxt(trafo_fn)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now we can project the polyline into our original image. This is where the exercise starts"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Exercise"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Solve the TODO items in `exercises/lane_detection/camera_geometry.py` which are labeled as **\"TODO step 1\"**.\n",
    "\n",
    "The cells below will help you check if your implementation is correct. You might want to read them before you start with your implementation."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Unit test"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# execute this cell to run unit tests on your implementation of step 1\n",
    "%cd ../../../\n",
    "!python -m code.tests.lane_detection.camera_geometry_unit_test 1\n",
    "%cd -"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Test by visual inspection"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "When you change the boolean below to `True`, your code will be run. Otherwise the sample solution will be run. The images that the code generates should be the same for your code and the sample solution."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "run_student_code = False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "import sys\n",
    "from pathlib import Path\n",
    "sys.path.append(str(Path('../../')))\n",
    "if run_student_code:\n",
    "    from exercises.lane_detection import camera_geometry\n",
    "else:\n",
    "    from solutions.lane_detection import camera_geometry"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "cg = camera_geometry.CameraGeometry()\n",
    "K = cg.intrinsic_matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "for boundary_polyline in [boundary_gt[:,0:3], boundary_gt[:,3:]]:\n",
    "    uv = camera_geometry.project_polyline(boundary_polyline, trafo_world_to_cam, K)\n",
    "    u,v = uv[:,0], uv[:,1]\n",
    "    plt.plot(u,v)\n",
    "plt.imshow(image);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Bonus information\n",
    "The image above is good, but not in the proper format if we want to use it to train a neural net for image segmentation.\n",
    "Here, I quickly show you how to get label images in the proper format. You can skip this section if you want."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def create_label_img(lb_left, lb_right):\n",
    "    label = np.zeros((512, 1024, 3))\n",
    "    colors = [[1, 1, 1], [2, 2, 2]]\n",
    "    for color, lb in zip(colors, [lb_left, lb_right]):\n",
    "        cv2.polylines(label, np.int32([lb]), isClosed=False, color=color, thickness=5)\n",
    "    label = np.mean(label, axis=2)  # collapse color channels to get gray scale\n",
    "    return label\n",
    "\n",
    "uv_left = camera_geometry.project_polyline(boundary_gt[:,0:3], trafo_world_to_cam, K)\n",
    "uv_right = camera_geometry.project_polyline(boundary_gt[:,3:], trafo_world_to_cam, K)\n",
    "\n",
    "label = create_label_img(uv_left, uv_right)\n",
    "plt.imshow(label, cmap=\"gray\");\n",
    "# cv2.imwrite(\"mylabel.png\", label)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Note that matplotlib's imshow rescales the intensity, which is why we can nicely see the lane boundaries here. If you would save the label as a png with `cv2.imwrite()` and would open it in an image viewing program it would look all black. That is because the maximum intensity is 255, and hence 0,1, and 2 all look black. This is not a problem, because the label image is intended for the deep learning model, not the human eye."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {},
 "nbformat": 4,
 "nbformat_minor": 0
}