{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Inverse perspective mapping"
   ]
  },
  {
   "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": [
    "## Exercise"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Solve the TODO items in `exercises/lane_detection/camera_geometry.py` which are labeled as **\"TODO step 2\"**.\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 2\n",
    "%cd ../../../\n",
    "!python -m code.tests.lane_detection.camera_geometry_unit_test 2\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": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import cv2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First we construct the pixel coordinates $(u,v)$ for the left lane boundary, in the same way that we did it in the chapter on image formation:"
   ]
  },
  {
   "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 = cv2.imread(image_fn)\n",
    "image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)\n",
    "\n",
    "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)\n",
    "\n",
    "cg = camera_geometry.CameraGeometry()\n",
    "K = cg.intrinsic_matrix\n",
    "\n",
    "left_boundary_3d_gt_world = boundary_gt[:,0:3]\n",
    "uv = camera_geometry.project_polyline(left_boundary_3d_gt_world, 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": [
    "Now we have image coordinates $(u,v)$ in our numpy array `uv`. Let us try to reconstruct the 3d coordinates using equation"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$$\n",
    "    \\begin{pmatrix} X_c \\\\ Y_c \\\\Z_c \\end{pmatrix} = \\frac{h}{ \\mathbf{n_c}^T \\mathbf{K}^{-1} (u,v,1)^T} \\mathbf{K}^{-1} \\begin{pmatrix} u \\\\ v \\\\ 1 \\end{pmatrix} \n",
    "$$ "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The relevant code is implemented in camera_geometry.py in the function `uv_to_roadXYZ_camframe()`."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Reconstruct the left boundary starting from the known u,v\n",
    "reconstructed_lb_3d_cam = []\n",
    "for u,v in uv:\n",
    "    xyz = cg.uv_to_roadXYZ_camframe(u,v)\n",
    "    reconstructed_lb_3d_cam.append(xyz)\n",
    "reconstructed_lb_3d_cam = np.array(reconstructed_lb_3d_cam)"
   ]
  },
  {
   "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_cam_to_world = np.linalg.inv(trafo_world_to_cam)\n",
    "reconstructed_lb_3d_world = map_between_frames(reconstructed_lb_3d_cam, trafo_cam_to_world)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# plot both ground truth and reconstructed left boundary 3d in X-Y-plane\n",
    "plt.plot(left_boundary_3d_gt_world[:,0], left_boundary_3d_gt_world[:,1], label=\"ground truth\")\n",
    "plt.plot(reconstructed_lb_3d_world[:,0], reconstructed_lb_3d_world[:,1], ls = \"--\", label=\"reconstructed\")\n",
    "plt.axis(\"equal\")\n",
    "plt.legend();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should see that the lines overlap. Finally, we can also do this comparison in the road frame instead of the world frame."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# compare ground truth and reconstructed boundary in road frame\n",
    "trafo_world_to_road = cg.trafo_cam_to_road @ trafo_world_to_cam\n",
    "left_boundary_3d_gt_road = map_between_frames(left_boundary_3d_gt_world, trafo_world_to_road)\n",
    "reconstructed_lb_3d_road = map_between_frames(reconstructed_lb_3d_cam, cg.trafo_cam_to_road)\n",
    "\n",
    "# plot both ground truth and reconstructed left boundary 3d in Z-(-X)-plane (which is X-Y in road iso 8855)\n",
    "plt.plot(left_boundary_3d_gt_road[:,2], -left_boundary_3d_gt_road[:,0], label=\"ground truth\")\n",
    "plt.plot(reconstructed_lb_3d_road[:,2], -reconstructed_lb_3d_road[:,0], ls = \"--\", label=\"reconstructed\")\n",
    "plt.axis(\"equal\")\n",
    "plt.legend();"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You should see that the lines overlap."
   ]
  }
 ],
 "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
}