{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Lane Detector"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In this exercise we will implement the polynomial fitting and then combine all functionality into one `LaneDetector` class"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "## 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": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "if colab_nb:\n",
    "  !pip install segmentation-models-pytorch\n",
    "  !pip install albumentations --upgrade"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Precompute the grid"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the book you have seen how the tensor `xyp` was computed. Its first two columns have the `x` and `y` values respectively, while the last column has the probability values. The `x` and `y` values will always be the same. Hence they only need to be computed once.\n",
    "This is what you should implement first. It is marked as \"TODO step 3\" in `code/exercises/lane_detection/camera_geometry.py`. Note that there is one additional modification to what you have seen in the book: The `cut_v` parameter. In the book the `(x,y,p)` triples were computed from all possible `u, v, p[v,u]`. Here you should restrict yourself to all `v` with `v>cut_v`. The idea is that pixels with low `v` values are too far away or even above the horizon, and hence should not be considered for fitting later. The other modification is of course that you do not need to compute an `xyp` tensor, since you have no probabilities given. You only precompute the first two columns of the `xyp` tensor.\n",
    "\n",
    "Once you implemented \"TODO step 3\", check whether your implementation is correct using the unit test:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# execute this cell to run unit tests on your implementation of step 3\n",
    "%cd ../../../\n",
    "!python -m code.tests.lane_detection.camera_geometry_unit_test 3\n",
    "%cd -"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Implement the LaneDetector class"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Your final step is to implement the LaneDetector class. \n",
    "\n",
    "1. Read the rest of this notebook. You will find places where it says \"TODO\" and you are asked to change something. Do not do this yet! For now, you should just see the sample solution at work.\n",
    "2. Go to `code/exercises/lane_detection/lane_detector.py` and implement the \"TODO\" items. \n",
    "3. Now it is time to test **your** lane detector. Go through all the cells below and execute them. Some cells will have a \"TODO\". Please resolve them, so that your lane detector is being run.\n",
    "\n",
    "Does your `LaneDetector` class work to your satisfaction? If not, debug and improve it!"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np \n",
    "import matplotlib.pyplot as plt\n",
    "from IPython import display\n",
    "display.set_matplotlib_formats('svg')\n",
    "from pathlib import Path\n",
    "import cv2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys\n",
    "sys.path.append(str(Path('../../')))\n",
    "# TODO: In the next two lines, change \"solutions\" to \"exercises\". Now your code will be executed here!\n",
    "from solutions.lane_detection.lane_detector import LaneDetector\n",
    "from solutions.lane_detection.camera_geometry import CameraGeometry\n",
    "cg = CameraGeometry()"
   ]
  },
  {
   "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\").absolute())\n",
    "image_arr = cv2.imread(image_fn)\n",
    "image_arr = cv2.cvtColor(image_arr, cv2.COLOR_BGR2RGB)\n",
    "plt.imshow(image_arr);"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Get lane boundaries from LaneDetector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# TODO: Change the next line(s), to create an instance of *your* LaneDetector\n",
    "model_path = Path(\"../../solutions/lane_detection/fastai_model.pth\")\n",
    "ld = LaneDetector(model_path=model_path)\n",
    "poly_left, poly_right = ld(image_fn)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# It should also be possible to pass the image as an array into the lane detector\n",
    "# The following assertions should not raise an AssertionError\n",
    "poly_left_2, poly_right_2 = ld(image_arr)\n",
    "np.testing.assert_allclose(poly_left, poly_left_2, rtol=1e-5)\n",
    "np.testing.assert_allclose(poly_right, poly_right_2, rtol=1e-5)\n",
    "# we are using `assert_allclose` to compare floating point numbers here."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Let's see how fast the lane detector works:\n",
    "%timeit poly_left, poly_right = ld(image_arr)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Get ground truth for lane boundaries"
   ]
  },
  {
   "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",
    "\n",
    "trafo_fn = image_fn.replace(\".png\", \"_trafo.txt\")\n",
    "trafo_world_to_cam = np.loadtxt(trafo_fn)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Map reconstructed left boundary into world reference frame\n",
    "def map_between_frames(points, trafo_matrix):\n",
    "    x,y,z = points[:,0], points[:,1], points[:,2]\n",
    "    homvec = np.stack((x,y,z,np.ones_like(x)))\n",
    "    return (trafo_matrix @ homvec).T\n",
    "\n",
    "trafo_world_to_road = cg.trafo_cam_to_road @ trafo_world_to_cam"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "left_boundary_3d_gt_world = boundary_gt[:,0:3]\n",
    "\n",
    "left_boundary_gt_road = map_between_frames(boundary_gt[:,0:3], trafo_world_to_road)\n",
    "right_boundary_gt_road = map_between_frames(boundary_gt[:,3:], trafo_world_to_road)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Plot LaneDetector output and ground truth"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# ground truth\n",
    "plt.plot(left_boundary_gt_road[:,2], -left_boundary_gt_road[:,0], label=\"ground truth left\")\n",
    "plt.plot(right_boundary_gt_road[:,2], -right_boundary_gt_road[:,0], label=\"ground truth right\")\n",
    "# LaneDetector\n",
    "x = np.arange(0,60,1)\n",
    "yl = poly_left(x)\n",
    "yr = poly_right(x)\n",
    "plt.plot(x,yl, ls = \"--\", label=\"LaneDector left\")\n",
    "plt.plot(x,yr, ls = \"--\", label=\"LaneDector right\")\n",
    "plt.legend()\n",
    "# TODO: You can also inspect the plot while commenting out the next line\n",
    "#plt.axis(\"equal\");"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the plot above, the LaneDetector should yield something close to the ground truth (less than 1m error along the y axis). "
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.11"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}