07_Stereo.ipynb

{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "PU831zdvOJ86"
      },
      "source": [
        "# Tutorial 07 - Stereo\n",
        "\n",
        "## Dr. David C. Schedl\n",
        "\n",
        "Note: this tutorial is geared towards students **experienced in programming** and aims to introduce you to **Computer Vision** techniques.\n",
        "\n",
        "\n",
        "Useful links:\n",
        "* General OpenCV Tutorials: https://docs.opencv2.org/master/d9/df8/tutorial_root.html\n",
        "* Image Processing in Pyhton: https://github.com/xn2333/OpenCV/blob/master/Seminar_Image_Processing_in_Python.ipynb\n",
        "* OpenCV Camera Calibration Tutorials: https://docs.opencv.org/master/d9/db7/tutorial_py_table_of_contents_calib3d.html\n",
        "\n",
        "\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "RFUPuyJVOJ88"
      },
      "source": [
        "# Table of Contents  \n",
        "\n",
        "- Initialization\n",
        "- Calibration\n",
        "- Stereo Calibration\n",
        "- Epipolar Geometry\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "YkE2YAvyOJ88"
      },
      "source": [
        "<a id=\"Initialization\"></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "weMMc2WZlsJc"
      },
      "source": [
        "# Initialization"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "zkUzZ6lWK_S5"
      },
      "source": [
        "As always let's import useful libraries, first. \n",
        "We will work with images today. So let's download some with `curl`.\n",
        "Let's define utility functions to display images, in Jupyter Notebooks. OpenCV's `imshow` does not work and matplotlib's `imshow` needs special treatment due to color channel handling (RGB vs. BGR)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "PtUm6wJFOJ89"
      },
      "outputs": [],
      "source": [
        "%%capture \n",
        "# %% capture suppress any output\n",
        "\n",
        "\n",
        "# import the libraries we use\n",
        "import os\n",
        "import cv2 # openCV\n",
        "import numpy as np\n",
        "import matplotlib.pyplot as plt\n",
        "from glob import glob\n",
        "\n",
        "\n",
        "\n",
        "if not os.path.isdir('data'):\n",
        "  os.mkdir('data')\n",
        "\n",
        " \n",
        "# downloading images from the official OpenCV Github repository!\n",
        "for lr in ['left','right']:\n",
        "  for i in range(14):\n",
        "    src = \"https://raw.githubusercontent.com/opencv/opencv/master/samples/data/{:}{:02d}.jpg\".format(lr, i+1)\n",
        "    dst = \"data/{:}{:02d}.jpg\".format(lr, i+1)\n",
        "    !curl -o {dst} {src}\n",
        "\n",
        "# utility function(s)\n",
        "def imshow(image, *args, **kwargs):\n",
        "    \"\"\"A replacement for cv2.imshow() for use in Jupyter notebooks using matplotlib.\n",
        "\n",
        "        Args:\n",
        "          image : np.ndarray. shape (N, M) or (N, M, 1) is an NxM grayscale image. shape\n",
        "            (N, M, 3) is an NxM BGR color image. \n",
        "    \"\"\"\n",
        "    if len(image.shape) == 3:\n",
        "      # Height, width, channels\n",
        "      # Assume BGR, do a conversion  \n",
        "      image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)\n",
        "\n",
        "    # Draw the image\n",
        "    plt.imshow(image, *args, **kwargs)\n",
        "    # We'll also disable drawing the axes and tick marks in the plot, since it's actually an image\n",
        "    plt.axis('off')\n",
        "    # Make sure it outputs\n",
        "    # plt.show()\n",
        "\n",
        "def splitfn(fn):\n",
        "    \"\"\" utility function to get the path, filename and the extionsion \n",
        "        from https://github.com/opencv/opencv/blob/master/samples/python/common.py\n",
        "    \"\"\"\n",
        "    path, fn = os.path.split(fn)\n",
        "    name, ext = os.path.splitext(fn)\n",
        "    return path, name, ext"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "44JU8cDEFMiZ"
      },
      "source": [
        "# Calibration\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "sOd3oa6hFOHf"
      },
      "source": [
        "## Finding checkerboard corners\n",
        "\n",
        "We can use `cv2.findChessboardCorners(img, pattern_size)` to find the checkerboard corners ($u,v$) in the image. The 3D coordinates of the corners are known, since they are known points on a plane. With `cv2.cornerSubPix` the image locations can be found with sub-pixel accuracy. "
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 575
        },
        "id": "tak9ODniA_l2",
        "outputId": "cbb2e090-6741-49c0-840f-41dd337f6b19"
      },
      "outputs": [],
      "source": [
        "\n",
        "def processImage(img):\n",
        "    if img is None:\n",
        "        print(\"Image is None!\")\n",
        "        return None\n",
        "\n",
        "    # parameters of the checkerboard\n",
        "    square_size = 1.0\n",
        "\n",
        "    pattern_size = (9, 6)\n",
        "    pattern_points = np.zeros((np.prod(pattern_size), 3), np.float32)\n",
        "    pattern_points[:, :2] = np.indices(pattern_size).T.reshape(-1, 2)\n",
        "    pattern_points *= square_size\n",
        "\n",
        "    found, corners = cv2.findChessboardCorners(img, pattern_size)\n",
        "    if found: # refine corner with sub-pixel accuracy\n",
        "        term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1)\n",
        "        corners = cv2.cornerSubPix(img, corners, (5, 5), (-1, -1), term)\n",
        "\n",
        "    if not found:\n",
        "        print('chessboard not found')\n",
        "        return None\n",
        "\n",
        "    return (corners.reshape(-1, 2), pattern_points)\n",
        "\n",
        "img = cv2.imread( './data/left14.jpg', cv2.IMREAD_GRAYSCALE )\n",
        "(corners, pattern_points) = processImage( img )\n",
        "#print(corners, pattern_points)\n",
        "\n",
        "plt.figure(figsize=(15,10))\n",
        "imshow(img, cmap='gray')\n",
        "plt.plot(corners[:,0], corners[:,1], 'rx')\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fWPwOQ0wcy37"
      },
      "source": [
        "## Computing the intrinsic, extrinsic and distortion parameters\n",
        "\n",
        "With `cv2.calibrateCamera(obj_points, img_points, (w, h)` and the 2D to 3D correspondences we can compute the intrinsic, extrinsic and distortion parameters. "
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "5svbl5zXA_l3",
        "outputId": "2eab2e81-63a9-4e85-a131-005c4f9b3865"
      },
      "outputs": [],
      "source": [
        "# process all images\n",
        "img_mask = './data/left??.jpg'  # default\n",
        "img_names = sorted(glob(img_mask))\n",
        "\n",
        "images = [ cv2.imread(fn,cv2.IMREAD_GRAYSCALE) for fn in img_names ]\n",
        "images = [ x for x in images if x is not None ] # filter empty images\n",
        "h, w = images[0].shape[:2]\n",
        "\n",
        "# find corners for all images\n",
        "chessboards = [processImage( img ) for img in images] # process all images\n",
        "chessboards = [x for x in chessboards if x is not None] # filter images that did not work!\n",
        "img_points, obj_points = [],[]\n",
        "for (corners, pattern_points) in chessboards:\n",
        "    img_points.append(corners)\n",
        "    obj_points.append(pattern_points)\n",
        "\n",
        "# calculate camera distortion\n",
        "rms, camera_matrix, dist_coefs, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, (w, h), None, None)\n",
        "\n",
        "print(\"\\nRMS:\", rms)\n",
        "print(\"camera matrix:\\n\", camera_matrix)\n",
        "print(\"distortion coefficients: \", dist_coefs.ravel())\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "BxMKBo48dbZt"
      },
      "source": [
        "## Reprojection\n",
        "\n",
        "Let's verify the model by projection 3D points onto our checkerboard again. The root-mean-squared (RMS) error is the distance between the found points and the projected known 3D points. Ideally the RMS is very small.\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 575
        },
        "id": "7mJpLWljA_l3",
        "outputId": "ecd8e5a7-8b41-4062-fdb5-478542f42a22"
      },
      "outputs": [],
      "source": [
        "# let's verify the model by projection 3D points onto our checkerboard corners!\n",
        "\n",
        "img_id = 10\n",
        "img = images[img_id]\n",
        "#dst = cv2.undistort(img, camera_matrix, dist_coefs, None, camera_matrix)\n",
        "pattern_points = obj_points[0]\n",
        "\n",
        "imgpts, jac = cv2.projectPoints( pattern_points, rvecs[img_id], tvecs[img_id], camera_matrix, dist_coefs  )\n",
        "\n",
        "plt.figure(figsize=(15,10))\n",
        "imshow(img, cmap='gray')\n",
        "plt.plot(img_points[img_id][:,0], img_points[img_id][:,1], 'rx', label='corner')\n",
        "plt.plot(imgpts[:,:,0], imgpts[:,:,1], 'b+', label='projection')\n",
        "plt.legend()\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "jLg4uKfae0bs"
      },
      "source": [
        "## Undistort\n",
        "\n",
        "Furthermore, we can remove any lens distortions from the images. After undistorition the pinhole model can be applied."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 575
        },
        "id": "p2ARIJGrA_l4",
        "outputId": "e6cae754-3c2f-42ac-b441-bd5232e6b243"
      },
      "outputs": [],
      "source": [
        "# let's remove the lens distortions\n",
        "\n",
        "img_id = 10 # pick any image you want to display\n",
        "img = images[img_id]\n",
        "\n",
        "# remove the the lens distortions\n",
        "dst = cv2.undistort(img, camera_matrix, dist_coefs, None, camera_matrix)\n",
        "pattern_points = obj_points[0]\n",
        "\n",
        "# projecting points can be done without any distortions parameters, now\n",
        "imgpts, jac = cv2.projectPoints( pattern_points, rvecs[img_id], tvecs[img_id], camera_matrix, None  )\n",
        "\n",
        "# the original detections were done in the distorted image, so they also require correction\n",
        "dst_points = cv2.undistortPoints( img_points[img_id], camera_matrix, dist_coefs, None, camera_matrix )\n",
        "\n",
        "plt.figure(figsize=(15,10))\n",
        "imshow(dst, cmap='gray')\n",
        "plt.plot(dst_points[:,:,0], dst_points[:,:,1], 'rx', label='corner')\n",
        "plt.plot(imgpts[:,:,0], imgpts[:,:,1], 'b+', label='projection')\n",
        "plt.legend()\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Hor7ylpneuWf"
      },
      "source": [
        "##  ⌨️ Try it yourself: Manual projection\n",
        "\n",
        "Try to project the points without using `cv2.projectPoints`. Note that the pinhole model can be applied with the undistorted images. So 3D points $X$ can be projected into the image $x$ by \n",
        "$$\n",
        "x = K [R \\  t] X\n",
        "$$\n",
        "or\n",
        "$$\n",
        "x = K P X\n",
        "$$ \n",
        "where $P = [R \\ t]$ (3x4 matrix). \n",
        "Remember that $x$ and $X$ uses homogeneous coordinates. Before plotting them on the image they need to be converted to non-homogeneous coordinates. "
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 575
        },
        "id": "Q2x58oFxfb8P",
        "outputId": "61029219-b4ec-4846-ce16-c27a813cdef3"
      },
      "outputs": [],
      "source": [
        "pts3D = pattern_points\n",
        "K = camera_matrix # intrinsic (3x3)\n",
        "R,_ = cv2.Rodrigues(rvecs[img_id])# extrinsic matrix as 3x3 matrix\n",
        "t = tvecs[img_id] # extrinsic translation (3D vector)\n",
        "\n",
        "P = np.hstack((R,t))\n",
        "\n",
        "# projecting points can be done without any distortions parameters \n",
        "uvs = np.zeros(shape=(2,0))\n",
        "for pt3D in pts3D:\n",
        "  \n",
        "  \n",
        "  uv = [w/2,h/2] # ToDo: change this line!\n",
        "\n",
        "  uvs = np.column_stack((uvs,uv)) # store\n",
        "\n",
        "# display\n",
        "plt.figure(figsize=(15,10))\n",
        "imshow(dst, cmap='gray')\n",
        "plt.plot(dst_points[:,:,0], dst_points[:,:,1], 'rx', label='found corner')\n",
        "plt.plot(uvs[0], uvs[1], 'b+', label='your projection')\n",
        "plt.legend()\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "bWIlTuL65ynr"
      },
      "source": [
        "# (A poor man's) Augmented Reality (AR)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "OlXmT1MZ6EF7"
      },
      "source": [
        "Let's put some 3D content on our checkerboard. We assume the checkerboard is the ground plane. "
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 609
        },
        "id": "Zty1HhsFA_l4",
        "outputId": "a7015d90-5875-469a-bacf-8bf393f54d7e"
      },
      "outputs": [],
      "source": [
        "# let's add a 3D content on our image, a debug axis\n",
        "\n",
        "img_id = 3 # select an image (0 to 12)\n",
        "img = images[img_id]\n",
        "dst = cv2.undistort(img, camera_matrix, dist_coefs, None, camera_matrix)\n",
        "pattern_points = obj_points[0]\n",
        "\n",
        "axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)\n",
        "axis_imgpts, jac = cv2.projectPoints( axis, rvecs[img_id], tvecs[img_id], camera_matrix, None  )\n",
        "print(axis_imgpts[0].ravel())\n",
        "\n",
        "origin = np.float32([[0,0,0]]).reshape(-1,3)\n",
        "origin_imgpts, jac = cv2.projectPoints( origin, rvecs[img_id], tvecs[img_id], camera_matrix, None  )\n",
        "print(origin_imgpts)\n",
        "\n",
        "def drawAxes(img, corners, imgpts):\n",
        "    corner = np.int32(corners[0].ravel())\n",
        "    img = cv2.line(img, corner, np.int32(imgpts[0].ravel()), (0,0,255), 5) # x -> red\n",
        "    img = cv2.line(img, corner, np.int32(imgpts[1].ravel()), (0,255,0), 5) # y -> green\n",
        "    img = cv2.line(img, corner, np.int32(imgpts[2].ravel()), (255,0,0), 5) # z -> blue\n",
        "    return img\n",
        "\n",
        "vis = cv2.cvtColor( dst, cv2.COLOR_GRAY2BGR)\n",
        "drawAxes(vis, origin_imgpts, axis_imgpts)\n",
        "\n",
        "# display\n",
        "plt.figure(figsize=(15,10))\n",
        "imshow(vis, cmap='gray')\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "JzcIsLBg6OAL"
      },
      "source": [
        "##  ⌨️ Try it yourself: Your 3D object\n",
        "\n",
        "Draw your own 3D object by drawing 3D lines and polygons on our image. \n",
        "For example, you can draw a colored cube, a house, a car, ... \n",
        "\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 575
        },
        "id": "0iMaVvOX6pGR",
        "outputId": "cb652ce9-ef16-4fd3-8239-96a968287d91"
      },
      "outputs": [],
      "source": [
        "img_id = 11 # select an image (0 to 12)\n",
        "img = images[img_id]\n",
        "dst = cv2.undistort(img, camera_matrix, dist_coefs, None, camera_matrix)\n",
        "vis = cv2.cvtColor( dst, cv2.COLOR_GRAY2BGR)\n",
        "\n",
        "def draw3DPolygon( vis, axis, color, filled, Rv, tv, K, dist_coefs=None ):\n",
        "\n",
        "  axis = np.asarray(axis, dtype=np.float32).reshape(-1,3) # make sure it has the correct format\n",
        "  axis_imgpts, jac = cv2.projectPoints( axis, Rv, tv, K, dist_coefs )\n",
        "\n",
        "  if filled: # fill the polygon or only draw the contour \n",
        "    thickness = -3\n",
        "  else:\n",
        "    thickness = 3\n",
        "\n",
        "\n",
        "  vis = cv2.drawContours(vis, [np.asarray(axis_imgpts.reshape(-1,2),dtype=np.int32)],-1,color,thickness)\n",
        "\n",
        "# draw filled polygon\n",
        "draw3DPolygon( vis, [[0,0,0], [3,0,0], [3,3,0], [0,3,0]], (0,0,255), True, rvecs[img_id], tvecs[img_id], camera_matrix, None )\n",
        "# draw a line\n",
        "draw3DPolygon( vis, [[1.5,1.5,-3], [1.5,1.5,0]], (255,0,0), False, rvecs[img_id], tvecs[img_id], camera_matrix, None )\n",
        "# draw non-filled contour\n",
        "draw3DPolygon( vis, [[0,0,-3], [3,0,-3], [3,3,-3], [1.5,1.5,-3], [0,3,-3]], (0,255,0), False, rvecs[img_id], tvecs[img_id], camera_matrix, None )\n",
        "\n",
        "\n",
        "# display\n",
        "plt.figure(figsize=(15,10))\n",
        "imshow(vis, cmap='gray')\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "K8T1fSZbA_l5"
      },
      "source": [
        "# Stereo Calibration\n",
        "\n",
        "Let's calibrate two cameras that record simultaneously. So we always get a pair of images (left and right)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 522
        },
        "id": "t9lCqvMZA_l5",
        "outputId": "933f80f6-54da-48d8-faf5-0cfd45554164"
      },
      "outputs": [],
      "source": [
        "left = cv2.imread( './data/left14.jpg', cv2.IMREAD_GRAYSCALE )\n",
        "(left_corners, left_pattern_points) = processImage( left )\n",
        "right = cv2.imread( './data/right14.jpg', cv2.IMREAD_GRAYSCALE )\n",
        "(right_corners, right_pattern_points) = processImage( right )\n",
        "\n",
        "\n",
        "plt.figure(figsize=(25,10))\n",
        "plt.subplot(121), plt.title('left')\n",
        "imshow(left, cmap='gray')\n",
        "plt.plot(left_corners[:,0], left_corners[:,1], 'rx')\n",
        "\n",
        "plt.subplot(122), plt.title('right')\n",
        "imshow(right, cmap='gray')\n",
        "plt.plot(right_corners[:,0], right_corners[:,1], 'gx')\n",
        "\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "7i2F7CgNZxHI"
      },
      "source": [
        "We calibrated the left camera already. Let's do the same for the right camera."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "LaGTFJI7A_l5",
        "outputId": "8c39d545-d22f-4e71-ba13-8e053cc8c4ce"
      },
      "outputs": [],
      "source": [
        "# the previous data is for the left image\n",
        "left_images = images\n",
        "left_img_points = img_points\n",
        "left_cam_mat, left_dist_coefs = camera_matrix, dist_coefs\n",
        "\n",
        "\n",
        "# process right images\n",
        "img_mask = './data/right??.jpg'  # default\n",
        "img_names = sorted(glob(img_mask))\n",
        "\n",
        "right_images = [ cv2.imread(fn,cv2.IMREAD_GRAYSCALE) for fn in img_names ]\n",
        "right_images = [ x for x in right_images if x is not None ] # filter empty images\n",
        "assert len(right_images)==len(left_images), \"the number of left and right images does not match!\"\n",
        "\n",
        "# find corners for all images\n",
        "chessboards = [processImage( img ) for img in right_images] # process all images\n",
        "chessboards = [x for x in chessboards if x is not None] # filter images that did not work!\n",
        "right_img_points = []\n",
        "for (corners, pattern_points) in chessboards:\n",
        "    right_img_points.append(corners)\n",
        "\n",
        "# calculate camera distortion\n",
        "rms, right_cam_mat, right_dist_coefs, rvecs, tvecs = cv2.calibrateCamera(obj_points, right_img_points, (w, h), None, None)\n",
        "\n",
        "print(\"-RIGHT CAMERA-\\nRMS:\", rms)\n",
        "print(\"camera matrix:\\n\", right_cam_mat)\n",
        "print(\"distortion coefficients: \", right_dist_coefs.ravel())\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "rMS4dsUVZ2Wv"
      },
      "source": [
        "## Relative Transformation, Fundamental and Essential Matrix\n",
        "OpenCV provides a `stereoCalibrate` function that computes the relative rotation and translation of the two cameras."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "864QzP5AA_l5",
        "outputId": "ee921b7b-c1e7-4d06-f204-0e34b0df4979"
      },
      "outputs": [],
      "source": [
        "# calibrate the stereo setup, with the individual matrices and distortion coefficients of left and right\n",
        "rms, left_cam_mat, left_dist_coeffs, right_cam_mat, right_dist_coeffs, R, t, E, F \\\n",
        "    = cv2.stereoCalibrate( obj_points, left_img_points, right_img_points, left_cam_mat, left_dist_coefs, right_cam_mat, right_dist_coefs, (w,h) )\n",
        "    \n",
        "\n",
        "print(\"-STEREO-\\nRMS:\", rms)\n",
        "print(\"left camera matrix:\\n\", left_cam_mat)\n",
        "print(\"right camera matrix:\\n\", right_cam_mat)\n",
        "print(\"left distortion:  \", left_dist_coeffs.ravel())\n",
        "print(\"right distortion: \", right_dist_coeffs.ravel())\n",
        "\n",
        "print(\"relative rotation:\\n\", R)\n",
        "print(\"relative translation:\\n\", t)\n",
        "print(\"essential matrix:  \", E)\n",
        "print(\"fundamental matrix: \", F)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "W2BI9XxZIzWc"
      },
      "source": [
        "## Relation between $R, t, E$ and $F$\n",
        "\n",
        "### Essential Matrix $E$\n",
        "The Essential Matrix $E$ can be expressed by $R$ and $t$ as:\n",
        "\n",
        "$$ E = [t]_{\\times} R, $$\n",
        "\n",
        " where $[t]_{\\times}$ is the matrix representation of the cross product:\n",
        "\n",
        "$$\n",
        "[t]_{\\times} =\n",
        "\\begin{bmatrix}\n",
        "{0}&{-t_2}&{t_1}\\\\{t_2}&{0}&{-t_0}\\\\{-t_1}&{t_0}&{0}\n",
        "\\end{bmatrix}\n",
        "$$\n",
        "\n",
        "### Fundamental matrix $F$\n",
        "The fundamental matrix $F$ can be computed by $E$ and the two camera matrices $K$ and $K'$ by:\n",
        "$$\n",
        "F = K'^{-T} E K^{-1},\n",
        "$$\n",
        "where $K'^{-T}$ refers to the inverse and transpose of the second intrinsic camera matrix.\n",
        "\n",
        "Note that $F$ and $E$ might be scaled by a scalar (e.g., $sF$), which, however, does not influence the epipolar geomtry.\n",
        "\n",
        "Furthermore, note that typically the fundamental matrix $F$ is estimated and then the intrinsics and the essential matrix $E$ are estimated from $F$. \n",
        "By inverting the equation above we get:\n",
        "$$\n",
        "E = K'^{\\ T} F K\n",
        "$$\n",
        "\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "HmSu5lynefxz",
        "outputId": "dc385c35-12a8-4a32-9bc9-8b038087cc91"
      },
      "outputs": [],
      "source": [
        "# The essential matrix\n",
        "print( \"E = [t]x * R: \", np.cross(np.eye(3), t.ravel()) @ R )\n",
        "\n",
        "# The fundamental matrix expressed by K1, K2 and E\n",
        "_F = np.linalg.inv(right_cam_mat).transpose() @ E @ np.linalg.inv(left_cam_mat)\n",
        "print( \"F: \", (_F / _F[-1,-1]) ) # normalize\n",
        "\n",
        "# Vice versa, the essential matrix can be computed by F, K1 and K2\n",
        "_E = ( right_cam_mat.transpose() @ F @ left_cam_mat )\n",
        "print( \"E: \", _E / _E[-1,-1]) # normalize"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "xnynfOK5A_l6"
      },
      "source": [
        "# Epipolar Geometry"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "bTEK0hU8Qjg1"
      },
      "source": [
        "Let's compute the epipolar lines for the checkerboard corner points in both left and right images. \n",
        "Since the cameras' image axes are almost parallel the epipolar lines are almost parallel as well."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 332
        },
        "id": "OaCo_6bDA_l6",
        "outputId": "d347011a-1df4-497f-87da-9b422377950d"
      },
      "outputs": [],
      "source": [
        "# let's draw the epipolar lines\n",
        "\n",
        "img_id = 1 # select an image (0 to 12)\n",
        "left = left_images[img_id]\n",
        "left = cv2.undistort(left, left_cam_mat, left_dist_coeffs, None, left_cam_mat)\n",
        "left_points = cv2.undistortPoints( left_img_points[img_id], left_cam_mat, left_dist_coeffs, None, left_cam_mat )\n",
        "right = right_images[img_id]\n",
        "right = cv2.undistort(right, right_cam_mat, right_dist_coeffs, None, right_cam_mat)\n",
        "right_points = cv2.undistortPoints( right_img_points[img_id], right_cam_mat, right_dist_coeffs, None, right_cam_mat )\n",
        "\n",
        "def drawlines(img1,img2,lines,pts1,pts2):\n",
        "    ''' img1 - image on which we draw the epilines for the points in img2\n",
        "        lines - corresponding epilines '''\n",
        "    r,c = img1.shape\n",
        "    lines = lines.reshape(-1,3)\n",
        "    np.random.seed(42)\n",
        "    img1 = cv2.cvtColor(img1,cv2.COLOR_GRAY2BGR)\n",
        "    img2 = cv2.cvtColor(img2,cv2.COLOR_GRAY2BGR)\n",
        "    for r,pt1,pt2 in zip(lines,pts1,pts2):\n",
        "        color = tuple(np.random.randint(0,255,3).tolist())\n",
        "        x0,y0 = map(int, [0, -r[2]/r[1] ])\n",
        "        x1,y1 = map(int, [c, -(r[2]+r[0]*c)/r[1] ])\n",
        "        img1 = cv2.line(img1, (x0,y0), (x1,y1), color,1)\n",
        "        img1 = cv2.circle(img1,np.int32(pt1.ravel()),5,color,-1)\n",
        "        img2 = cv2.circle(img2,np.int32(pt2.ravel()),5,color,-1)\n",
        "    return img1,img2\n",
        "\n",
        "left_lines = cv2.computeCorrespondEpilines(right_points, 2, F)\n",
        "left_vis, _ = drawlines( left, right, left_lines, left_points, right_points )\n",
        "\n",
        "right_lines = cv2.computeCorrespondEpilines(left_points, 1, F)\n",
        "right_vis, _ = drawlines( right, left, right_lines, right_points, left_points )\n",
        "\n",
        "# display\n",
        "plt.figure(figsize=(15,10))\n",
        "plt.subplot(121), plt.title( 'left' )\n",
        "imshow(left_vis, cmap='gray')\n",
        "plt.subplot(122), plt.title( 'right' )\n",
        "imshow(right_vis, cmap='gray')\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "1yR9Uvy9wZwn"
      },
      "source": [
        "# Stereo Image Rectification"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "X6YaCuUfwkMM"
      },
      "source": [
        "Reproject image planes onto a common plane parallel to the line between camera centers. \n",
        "Epipolar lines are horizontal scanlines after this transformation.\n",
        "We can use `cv2.stereoRectify` to compute the desired transfromation matrices.\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 318
        },
        "id": "WbaP3hSesRxh",
        "outputId": "6558768f-6a56-40f3-91cd-838b085fb6b1"
      },
      "outputs": [],
      "source": [
        "img_id = 1\n",
        "\n",
        "R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(left_cam_mat, left_dist_coeffs, right_cam_mat, right_dist_coeffs,(w,h), R, t)\n",
        "\n",
        "# left\n",
        "map1x, map1y = cv2.initUndistortRectifyMap(left_cam_mat, left_dist_coeffs, R1, P1, (w, h), cv2.CV_32FC1)\n",
        "rleft = cv2.remap( left_images[img_id], map1x, map1y, cv2.INTER_LINEAR )\n",
        "\n",
        "# right\n",
        "map2x, map2y = cv2.initUndistortRectifyMap(right_cam_mat, right_dist_coeffs, R2, P2, (w, h), cv2.CV_32FC1)\n",
        "rright = cv2.remap( right_images[img_id], map2x, map2y, cv2.INTER_LINEAR )\n",
        "\n",
        "# display\n",
        "plt.figure(figsize=(15,10))\n",
        "plt.subplot(121), plt.title( 'rectified left' )\n",
        "imshow(rleft, cmap='gray')\n",
        "for y in range(0,h,50):\n",
        "  plt.plot([0,w],[y,y])\n",
        "plt.subplot(122), plt.title( 'rectified right' )\n",
        "imshow(rright, cmap='gray')\n",
        "for y in range(0,h,50):\n",
        "  plt.plot([0,w],[y,y])\n",
        "plt.show()\n"
      ]
    }
  ],
  "metadata": {
    "colab": {
      "name": "07_Stereo.ipynb",
      "provenance": [],
      "toc_visible": true
    },
    "interpreter": {
      "hash": "3e2b6404eb79c0fba1a251907aa00d26891e411eaa7ffd3f13ecfdd311f8c56c"
    },
    "kernelspec": {
      "display_name": "Python 3.8.10 64-bit (windows store)",
      "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.9.12"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}