Visual_Fusion_Late_Starter.ipynb

{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "colab": {
      "name": "Visual Fusion Late - Starter.ipynb",
      "private_outputs": true,
      "provenance": [],
      "collapsed_sections": [
        "KjNKW96LN2_L",
        "fYaVZiW5N6io",
        "wr79FMSMpT8E"
      ],
      "authorship_tag": "ABX9TyN16w37MQEJ11yEvHaQXGE0",
      "include_colab_link": true
    },
    "kernelspec": {
      "name": "python3",
      "display_name": "Python 3"
    },
    "language_info": {
      "name": "python"
    }
  },
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "view-in-github",
        "colab_type": "text"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/Jeremy26/visual_fusion_course/blob/main/Visual_Fusion_Late_Starter.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "6V2BxA_xar00"
      },
      "source": [
        "# Welcome to the Late Fusion Project\n",
        "\n",
        "Before we start, acknowledgement to this repo: https://github.com/kuixu/kitti_object_vis. This course has been based on this repo after seeing the great results and code! <p>\n",
        "\n",
        "We'll use the [KITTI Dataset](http://www.cvlibs.net/datasets/kitti/setup.php) to collect the Point Clouds, Images, and Calibration parameters. <p>\n",
        "\n",
        "After loading data from the dataset, our Late fusion process will happen in 5 steps:\n",
        "1.   **Detect Obstacles in 2D** with the Camera\n",
        "2.   **Detect Obstacles in 3D** with the LiDAR\n",
        "3.   **Project the 3D Obstacles** in the Image (3D Bounding Boxes)\n",
        "4.   **Fuse the 3D Bounding Box (LiDAR) with the 2D Bounding Box (Camera)**\n",
        "5.   **Create an ultimate Fused Objects** and Show the results\n",
        "\n",
        "Are you ready? ✌🏼\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "j75tiLPCLZh1"
      },
      "source": [
        "##0 - Load the Data and Visualize it!"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "KjNKW96LN2_L"
      },
      "source": [
        "### Link Google Colab to Google Drive"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "tk-izanQH9iY"
      },
      "source": [
        "import os\n",
        "from google.colab import drive\n",
        "drive.mount('/content/drive', force_remount=False)\n",
        "\n",
        "os.chdir(\"/content/drive/My Drive/Think Autonomous/SDC Course/Visual Fusion\")\n",
        "!ls"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "fYaVZiW5N6io"
      },
      "source": [
        "### Import the necessary libraries"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "1L0u3x7HL_9U"
      },
      "source": [
        "!pip install open3d==0.12.0 # Version 12"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "BC_e1h8MICKX"
      },
      "source": [
        "import numpy as np\n",
        "import cv2\n",
        "import matplotlib.pyplot as plt\n",
        "import matplotlib.image as mpimg\n",
        "from IPython.display import Image\n",
        "import glob\n",
        "#import open3d as o3d\n",
        "import pandas as pd"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "wr79FMSMpT8E"
      },
      "source": [
        "### Load the Files\n",
        "\n",
        "\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "qP3-j5OAKhT2"
      },
      "source": [
        "image_files = sorted(glob.glob(\"data/img/*.png\"))\n",
        "point_files = sorted(glob.glob(\"data/velodyne/*.pcd\"))\n",
        "label_files = sorted(glob.glob(\"data/label/*.txt\"))\n",
        "calib_files = sorted(glob.glob(\"data/calib/*.txt\"))\n",
        "\n",
        "print(\"There are\",len(image_files),\"images\")\n",
        "index = 3\n",
        "image = cv2.cvtColor(cv2.imread(image_files[index]), cv2.COLOR_BGR2RGB)\n",
        "pcd_file = point_files[index]\n",
        "cloud = o3d.io.read_point_cloud(pcd_file)\n",
        "points= np.asarray(cloud.points)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "y_ZiGOIVMKl9"
      },
      "source": [
        "### Visualize the Image"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "qn7cbMJYMMQn"
      },
      "source": [
        "f, (ax1) = plt.subplots(1, 1, figsize=(20,10))\n",
        "ax1.imshow(image)\n",
        "ax1.set_title('Image', fontsize=30)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Qmdm1q2mhKFm"
      },
      "source": [
        "### Visualize the Point Clouds"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "-sAnpfGoOIF5"
      },
      "source": [
        "!pip install pypotree #https://github.com/centreborelli/pypotree"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "_HqRVOwxJP8R"
      },
      "source": [
        "import pypotree \n",
        "cloudpath = pypotree.generate_cloud_for_display(points)\n",
        "pypotree.display_cloud_colab(cloudpath)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "tXbqd3IUB3V8"
      },
      "source": [
        "## 1 - Detect Obstacles in 2D with the Camera\n",
        "Here's what we should get from YOLO:\n",
        "* The **2D Bounding Box** coordinates (X1, Y1, X2, Y2)\n",
        "* The **class** of the object (Car, Pedestrian, ...)\n",
        "* The **confidence**\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "-XQhkCvZiRzo"
      },
      "source": [
        "### 1.1. Copy/Paste the function to use YOLO"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "P6YsCcOFITDT"
      },
      "source": [
        "!python3 -m pip install yolov4==2.0.2 # After Checking, YOLO 2.0.2 works without modifying anything. Otherwise keep 1.2.1"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "KIEr498eIPmL"
      },
      "source": [
        "from yolov4.tf import YOLOv4\n",
        "import tensorflow as tf\n",
        "import time\n",
        "\n",
        "yolo = YOLOv4(tiny=False)\n",
        "yolo.classes = \"Yolov4/coco.names\"\n",
        "yolo.make_model()\n",
        "yolo.load_weights(\"Yolov4/yolov4.weights\", weights_type=\"yolo\")\n",
        "\n",
        "def run_obstacle_detection(img):\n",
        "    start_time=time.time()\n",
        "    img = cv2.cvtColor(img, cv2.COLOR_BGR2RGB)\n",
        "    resized_image = yolo.resize_image(img)\n",
        "    # 0 ~ 255 to 0.0 ~ 1.0\n",
        "    resized_image = resized_image / 255.\n",
        "    #input_data == Dim(1, input_size, input_size, channels)\n",
        "    input_data = resized_image[np.newaxis, ...].astype(np.float32)\n",
        "\n",
        "    candidates = yolo.model.predict(input_data)\n",
        "\n",
        "    _candidates = []\n",
        "    result = img.copy()\n",
        "    for candidate in candidates:\n",
        "        batch_size = candidate.shape[0]\n",
        "        grid_size = candidate.shape[1]\n",
        "        _candidates.append(tf.reshape(candidate, shape=(1, grid_size * grid_size * 3, -1)))\n",
        "        #candidates == Dim(batch, candidates, (bbox))\n",
        "        candidates = np.concatenate(_candidates, axis=1)\n",
        "        #pred_bboxes == Dim(candidates, (x, y, w, h, class_id, prob))\n",
        "        pred_bboxes = yolo.candidates_to_pred_bboxes(candidates[0], iou_threshold=0.35, score_threshold=0.40)\n",
        "        pred_bboxes = pred_bboxes[~(pred_bboxes==0).all(1)] #https://stackoverflow.com/questions/35673095/python-how-to-eliminate-all-the-zero-rows-from-a-matrix-in-numpy?lq=1\n",
        "        pred_bboxes = yolo.fit_pred_bboxes_to_original(pred_bboxes, img.shape)\n",
        "        exec_time = time.time() - start_time\n",
        "        #print(\"time: {:.2f} ms\".format(exec_time * 1000))\n",
        "        result = yolo.draw_bboxes(img, pred_bboxes)\n",
        "        result = cv2.cvtColor(result, cv2.COLOR_BGR2RGB)\n",
        "    return result, pred_bboxes\n"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "XrJXQelVJOhE"
      },
      "source": [
        "result, pred_bboxes = run_obstacle_detection(image)\n",
        "fig_camera = plt.figure(figsize=(14, 7))\n",
        "ax_lidar = fig_camera.subplots()\n",
        "ax_lidar.imshow(result)\n",
        "plt.show()"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "gSFYVDTiiYsB"
      },
      "source": [
        "### 1.2 - Put these results into \"Object2D\" and build a list of camera objects"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "-Q_6uZDePqQC"
      },
      "source": [
        "-- TODO-- Build the Object 2D class."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "OqR_PMR-Bzi3"
      },
      "source": [
        "class Object2D(object):\n",
        "    def __init__(self, box2D, w, h):\n",
        "        self.xmin = \n",
        "        self.ymin = \n",
        "        self.xmax = \n",
        "        self.ymax = \n",
        "        self.bbox = \n",
        "        self.category =\n",
        "        self.confidence = \n",
        "\n",
        "def fill_2D_obstacles(result, pred_bboxes):\n",
        "    return [Object2D(box, result.shape[1], result.shape[0]) for box in pred_bboxes]"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "64awANwbQoOp"
      },
      "source": [
        "list_of_2d_objects = fill_2D_obstacles(result, pred_bboxes)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "OpCwI9jfCESf"
      },
      "source": [
        "## 2 - Detect Obstacles in 3D\n",
        "\n",
        "In this part, we are supposed to detect objects in 3D, and then convert that to the camera frame. I won't cover 3D object detection, as we can't really use it easily as a Black Box like YOLO. I invite you to take my course [Point Clouds Fast Course: Introduction to 3D Perception](https://courses.thinkautonomous.ai/point-clouds) to learn more about the process.\n",
        "\n",
        "<br>\n",
        "\n",
        "👉 **To simplify the overall process, we're going to directly work with the XYZ Position and Bounding Box dimensions in the camera frame.** Consider that engineers have:\n",
        "*   Detected Obstacles in 3D (X,Y,Z)\n",
        "*   Build a Bounding Box in 3D (W,H,L)\n",
        "*   Computed the orientation of that Bounding Box (Yaw Angle)\n",
        "\n",
        "👉 When they are detecting these, it's all in the Velodyne Frame.\n",
        "Then, we apply a conversion to the camera frame; exactly like when we projected the point clouds from the velodyne frame to the camera frame.\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "7VzAPLCxC9ti"
      },
      "source": [
        "def read_label(label_filename):\n",
        "    lines = [line.rstrip() for line in open(label_filename)]\n",
        "    objects = [Object3d(line) for line in lines if line.split(\" \")[0]!=\"DontCare\"]\n",
        "    return objects"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "cfUnl50tPyds"
      },
      "source": [
        "--- TODO --- Build the Object3D Class"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "N-V4g0juCCRH"
      },
      "source": [
        "class Object3d(object):\n",
        "    \"\"\" 3d object label \"\"\"\n",
        "    def __init__(self, label_file_line):\n",
        "        data = label_file_line.split(\" \")\n",
        "        data[1:] = [float(x) for x in data[1:]]\n",
        "\n",
        "        # extract 3d bounding box information\n",
        "        self.h = \n",
        "        self.w = \n",
        "        self.l = \n",
        "        self.t = \n",
        "        self.ry =\n",
        "\n",
        "        self.xmin = \n",
        "        self.xmax = \n",
        "        self.ymin = \n",
        "        self.ymax = \n",
        "        self.bbox2d = \n",
        "        self.bbox3d = "
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "gN3MLWAPkrsO"
      },
      "source": [
        "list_of_3d_objects = read_label(label_files[index])\n",
        "for obj3d in list_of_3d_objects:\n",
        "    print(\"Object Lateral Position (X), Height(Y), Distance (Z) :\"+ str(obj3d.t))"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "urf48yYXSFh6"
      },
      "source": [
        "Again, I invite you to take my course [Point Clouds Fast Course: Introduction to 3D Perception](https://courses.thinkautonomous.ai/point-clouds) to learn more about the 3D Perception process."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "iCkYAhYjR3ZM"
      },
      "source": [
        "## 3 - Project the 3D Obstacle to the Image (3D Bounding Box)\n",
        "\n",
        "From these values, we need to build a 3D Bounding Box in the Image Frame. We'll then fuse that Bounding Box with the Camera boxes.<p>\n",
        "\n",
        "Here's what we'll do:\n",
        "\n",
        "*   **Build a LiDAR2Camera Object**\n",
        "*   Build a function to **Project a 3D Point to a 2D Image**\n",
        "*   **Build the 3D Bounding Box and project it in the 2D Image**\n",
        "*   **Draw the Box** for Visualization\n",
        "\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "PRMYP2mKc_7G"
      },
      "source": [
        "### 3.1 - Create a LiDAR2Camera object and fill it with the P value from the camera calibration"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "6w3x3UrIVjoY"
      },
      "source": [
        "class LiDAR2Camera(object):\n",
        "    def __init__(self, calib_file):\n",
        "        self.P = np.reshape(self.read_calib_file(calib_file)[\"P2\"], [3,4])\n",
        "    \n",
        "    def read_calib_file(self, filepath):\n",
        "        \"\"\" Read in a calibration file and parse into a dictionary.\n",
        "        Ref: https://github.com/utiasSTARS/pykitti/blob/master/pykitti/utils.py\n",
        "        \"\"\"\n",
        "        data = {}\n",
        "        with open(filepath, \"r\") as f:\n",
        "            for line in f.readlines():\n",
        "                line = line.rstrip()\n",
        "                if len(line) == 0:\n",
        "                    continue\n",
        "                key, value = line.split(\":\", 1)\n",
        "                # The only non-float values in these files are dates, which\n",
        "                # we don't care about anyway\n",
        "                try:\n",
        "                    data[key] = np.array([float(x) for x in value.split()])\n",
        "                except ValueError:\n",
        "                    pass\n",
        "        return data"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "1sZKqHjLdcZw"
      },
      "source": [
        "### 3.2 - Create a function to project 3D points in 2D\n",
        "\n",
        "In Early Fusion, we were doing the conversion from the LiDAR to the camera, and then from the camera to the image. *Here, you already have the first step implemented*; so you only need to deal with the conversion **from the camera to the image**.\n",
        "\n",
        "1.   Convert to Homogeneous Coordinates\n",
        "2.   Multiply it with P\n",
        "3.   Divide it with the last element and convert it back to the cartesian coordinates\n",
        "\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "7P7ZksoXP4T9"
      },
      "source": [
        "-- TODO -- Build a function that projects a point to an image"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "vL59YZFMatju"
      },
      "source": [
        "def project_to_image(self, pts_3d):\n",
        "    \"\"\" Project 3d points to image plane.\n",
        "    \"\"\"\n",
        "    pass\n",
        "\n",
        "LiDAR2Camera.project_to_image = project_to_image"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "y685v3kMeBRo"
      },
      "source": [
        "### 3.3 - Compute the 3D Bounding Box from the Values\n",
        "\n",
        "Create a function that, given a 3D Object (X,Y,Z, W,H,L, RY), outputs a 3D Bounding Box (in the camera frame)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "6V-7pTUSQMwe"
      },
      "source": [
        "--- TODO -- Code a function that builds a 3D Bounding Box"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "42mQrnFRa2Fk"
      },
      "source": [
        "def roty(t):\n",
        "    \"\"\" Rotation about the y-axis. \"\"\"\n",
        "    c = np.cos(t)\n",
        "    s = np.sin(t)\n",
        "    return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])\n",
        "\n",
        "def compute_box_3d(self,obj):\n",
        "        \"\"\" Projects the 3d bounding box into the image plane.\n",
        "            Returns:\n",
        "                corners_2d: (8,2) array in left image coord.\n",
        "                corners_3d: (8,3) array in in rect camera coord.\n",
        "        \"\"\"\n",
        "        # compute rotational matrix around yaw axis\n",
        "        R = \n",
        "        # 3d bounding box dimensions\n",
        "        l = \n",
        "        w = \n",
        "        h = \n",
        "\n",
        "        # 3d bounding box corners\n",
        "        x_corners = \n",
        "        y_corners = \n",
        "        z_corners = \n",
        "\n",
        "        # rotate and translate 3d bounding box\n",
        "        #corners_3d = np.vstack([x_corners, y_corners, z_corners])\n",
        "        corners_3d = np.dot(R, np.vstack([x_corners, y_corners, z_corners]))\n",
        "        corners_3d[0, :] = corners_3d[0, :] + obj.t[0]\n",
        "        corners_3d[1, :] = corners_3d[1, :] + obj.t[1]\n",
        "        corners_3d[2, :] = corners_3d[2, :] + obj.t[2]\n",
        "\n",
        "        # only draw 3d bounding box for objs in front of the camera\n",
        "        if np.any(corners_3d[2, :] < 0.1):\n",
        "            corners_2d = None\n",
        "            return corners_2d\n",
        "\n",
        "        # project the 3d bounding box into the image plane\n",
        "        corners_2d = self.project_to_image(np.transpose(corners_3d))\n",
        "        \n",
        "        return corners_2d\n",
        "\n",
        "LiDAR2Camera.compute_box_3d = compute_box_3d"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "_DZVQ4XbdLmy"
      },
      "source": [
        "### 3.4 - Create a function to draw a bounding box in 3D, and another one to draw that same bounding box in 2D"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "ZGTQFTCqZ6N2"
      },
      "source": [
        "def draw_projected_box3d(self,image, qs, color=(255, 0, 0), thickness=2):\n",
        "        \"\"\" Draw 3d bounding box in image\n",
        "            qs: (8,3) array of vertices for the 3d box in following order:\n",
        "                1 -------- 0\n",
        "            /|         /|\n",
        "            2 -------- 3 .\n",
        "            | |        | |\n",
        "            . 5 -------- 4\n",
        "            |/         |/\n",
        "            6 -------- 7\n",
        "        \"\"\"\n",
        "        qs = qs.astype(np.int32)\n",
        "        for k in range(0, 4):\n",
        "            # Ref: http://docs.enthought.com/mayavi/mayavi/auto/mlab_helper_functions.html\n",
        "            i, j = k, (k + 1) % 4\n",
        "            # use LINE_AA for opencv3\n",
        "            # cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness, cv2.CV_AA)\n",
        "            cv2.line(image, (qs[i, 0], qs[i, 1]), (qs[j, 0], qs[j, 1]), color, thickness)\n",
        "            i, j = k + 4, (k + 1) % 4 + 4\n",
        "            cv2.line(image, (qs[i, 0], qs[i, 1]), (qs[j, 0], qs[j, 1]), color, thickness)\n",
        "            i, j = k, k + 4\n",
        "            cv2.line(image, (qs[i, 0], qs[i, 1]), (qs[j, 0], qs[j, 1]), color, thickness)\n",
        "        return image\n",
        "\n",
        "LiDAR2Camera.draw_projected_box3d = draw_projected_box3d"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "xFLfgd8BQBAZ"
      },
      "source": [
        "-- TODO -- Code a function that projects a 3D Box into a 2D one"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "fXwxrs6ixK_U"
      },
      "source": [
        "def project_8p_to_4p(self, pts_2d):\n",
        "    pass\n",
        "\n",
        "LiDAR2Camera.project_8p_to_4p = project_8p_to_4p"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "u6C4HUqxxB95"
      },
      "source": [
        "def draw_projected_box2d(self, image, qs, color=(255,0,0), thickness=2):\n",
        "    return cv2.rectangle(image, (int(qs[0]), int(qs[1])), (int(qs[2]), int(qs[3])), (255,0,0),2)\n",
        "\n",
        "LiDAR2Camera.draw_projected_box2d = draw_projected_box2d"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "0rrDPISMeInc"
      },
      "source": [
        "### 3.5 - Apply the full process and Draw the Box for Visualization"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "YWJ8KOq5Wt9f"
      },
      "source": [
        "def get_image_with_bboxes(self,img, objects):\n",
        "    img2 = np.copy(img)\n",
        "    img3 = np.copy(img)\n",
        "    for obj in objects:\n",
        "        boxes = self.compute_box_3d(obj)\n",
        "        if boxes is not None:\n",
        "            obj.bbox3d = boxes\n",
        "            obj.bbox2d = self.project_8p_to_4p(boxes)\n",
        "            img2 = self.draw_projected_box2d(img2, obj.bbox2d) # Draw the 2D Bounding Box\n",
        "            img3 = self.draw_projected_box3d(img3, obj.bbox3d) # Draw the 3D Bounding Box\n",
        "    return img2, img3\n",
        "\n",
        "LiDAR2Camera.get_image_with_bboxes=get_image_with_bboxes"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "EVteucEMX4dM"
      },
      "source": [
        "lidar2cam = LiDAR2Camera(calib_files[index])\n",
        "lidar_2d, lidar_3d =lidar2cam.get_image_with_bboxes(image, list_of_3d_objects)\n",
        "\n",
        "f,(ax1, ax2)= plt.subplots(1,2, figsize=(30,20))\n",
        "ax1.imshow(lidar_2d)\n",
        "ax1.set_title('Image with 2D Bounding Boxes from LiDAR', fontsize=15)\n",
        "ax2.imshow(lidar_3d)\n",
        "ax2.set_title('Image with 3D Bounding Boxes from LiDAR', fontsize=15)\n"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "rKm1NBIjSTya"
      },
      "source": [
        "## 4 - Fuse the 3D Bounding Box (LiDAR) with the 2D Bounding Box (Camera)\n",
        "In that step, we have many possibilities, here's what we'll do:\n",
        "1.   Show both the LiDAR and the Camera boxes on the same images\n",
        "2.   Compute the IOU Metrics for each box\n",
        "3.   Send the IOU Matrix to the Hungarian Algorithm that outputs the matches\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "29JFudQbawQv"
      },
      "source": [
        "### 4.1 - Show both lidar and camera boxes"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "nzXR0h8KgA9F"
      },
      "source": [
        "res = yolo.draw_bboxes(lidar_2d, pred_bboxes)\n",
        "\n",
        "fig_fusion = plt.figure(figsize=(14, 7))\n",
        "ax_fusion = fig_fusion.subplots()\n",
        "ax_fusion.imshow(res)\n",
        "plt.show()"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "nrbhB602azub"
      },
      "source": [
        "### 4.2 - Compute the IOU Metrics for every box"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "45IGPYFpagKw"
      },
      "source": [
        "def box_iou(box1, box2):\n",
        "    \"\"\"\n",
        "    Computer Intersection Over Union cost\n",
        "    \"\"\"\n",
        "    xA = max(box1[0], box2[0])\n",
        "    yA = max(box1[1], box2[1])\n",
        "    xB = min(box1[2], box2[2])\n",
        "    yB = min(box1[3], box2[3])\n",
        "\n",
        "    inter_area = max(0, xB - xA + 1) * max(0, yB - yA + 1)\n",
        "    # Calculate the Union area by using Formula: Union(A,B) = A + B - Inter(A,B)\n",
        "    box1_area = (box1[2] - box1[0] + 1) * (box1[3] - box1[1] + 1) #abs((box1[3] - box1[1])*(box1[2]- box1[0]))\n",
        "    box2_area = (box2[2] - box2[0] + 1) * (box2[3] - box2[1] + 1) #abs((box2[3] - box2[1])*(box2[2]- box2[0]))\n",
        "    union_area = (box1_area + box2_area) - inter_area\n",
        "\n",
        "    # compute the IoU\n",
        "    iou = inter_area/float(union_area)\n",
        "    return iou"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "sQVtw7e7a5E2"
      },
      "source": [
        "### 4.3 - Send the IOU Matrix to the Hungarian Algorithm that outputs the mathes"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "S9AI0fIBQfTR"
      },
      "source": [
        "--- TODO -- Create the function to associate two bounding boxes\n",
        "\n",
        "Optional: Also return the boxes that don't match on each sensor"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "HFu9OK1qJrom"
      },
      "source": [
        "from scipy.optimize import linear_sum_assignment\n",
        "\n",
        "def associate(lidar_boxes, camera_boxes):\n",
        "    \"\"\"\n",
        "    LiDAR boxes will represent the red bounding boxes\n",
        "    Camera will represent the other bounding boxes\n",
        "    Function goal: Define a Hungarian Matrix with IOU as a metric and return, for each box, an id\n",
        "    \"\"\"\n",
        "    # Define a new IOU Matrix nxm with old and new boxes\n",
        "    iou_matrix = \n",
        "\n",
        "    # Go through boxes and store the IOU value for each box \n",
        "    # You can also use the more challenging cost but still use IOU as a reference for convenience (use as a filter only)\n",
        "    for i,lidar_box in enumerate(lidar_boxes):\n",
        "        for j,camera_box in enumerate(camera_boxes):\n",
        "            iou_matrix[i][j] = box_iou(lidar_box, camera_box)\n",
        "\n",
        "    # Call for the Hungarian Algorithm\n",
        "    hungarian_row, hungarian_col = \n",
        "    hungarian_matrix = np.array(list(zip(hungarian_row, hungarian_col)))\n",
        "\n",
        "    # Create new unmatched lists for old and new boxes\n",
        "    matches, unmatched_camera_boxes, unmatched_lidar_boxes = [], [], []\n",
        "\n",
        "    # Go through the Hungarian Matrix, if matched element has IOU < threshold (0.3), add it to the unmatched \n",
        "    # Else: add the match    \n",
        "    for h in hungarian_matrix:\n",
        "        if(iou_matrix[h[0],h[1]]>0.4):\n",
        "            matches.append(h.reshape(1,2))\n",
        "    \n",
        "    if(len(matches)==0):\n",
        "        matches = np.empty((0,2),dtype=int)\n",
        "    else:\n",
        "        matches = np.concatenate(matches,axis=0)\n",
        "    \n",
        "    return matches"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "ztfCWCyViySv"
      },
      "source": [
        "lidar_boxes = [obs.bbox2d for obs in list_of_3d_objects] # Simply get the boxes\n",
        "camera_boxes = [obs.bbox for obs in list_of_2d_objects]\n",
        "matches = associate(lidar_boxes, camera_boxes)\n",
        "\n",
        "print(matches)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "jTO2meZ4CFSD"
      },
      "source": [
        "## 5 - Create an ultimate Fused Object with all the values"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "x1fJTGTgShSm"
      },
      "source": [
        "class FusedObject(object):\n",
        "    def __init__(self, bbox2d, bbox3d, category, t, confidence):\n",
        "        self.bbox2d = bbox2d\n",
        "        self.bbox3d = bbox3d\n",
        "        self.category = category\n",
        "        self.t = t\n",
        "        with open(\"Yolov4/classes.txt\",'rt') as f:\n",
        "            classes = f.read().rstrip('\\n').split('\\n')\n",
        "        self.class_ = classes[category] "
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "I0u0IyPqcmWV"
      },
      "source": [
        "def build_fused_object(list_of_2d_objects, list_of_3d_objects, matches, image):\n",
        "    \"Input: Image with 3D Boxes already drawn\"\n",
        "    final_image = image.copy()\n",
        "    list_of_fused_objects = []\n",
        "    for match in matches:\n",
        "        fused_object = FusedObject(list_of_2d_objects[match[1]].bbox, list_of_3d_objects[match[0]].bbox3d, \n",
        "                                   list_of_2d_objects[match[1]].category,list_of_3d_objects[match[0]].t,\n",
        "                                   list_of_2d_objects[match[1]].confidence)\n",
        "        cv2.putText(final_image, '{0:.2f} m'.format(fused_object.t[2]), (int(fused_object.bbox2d[0]),int(fused_object.bbox2d[1])), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (200, 200, 100), 3, cv2.LINE_AA)    \n",
        "        cv2.putText(final_image, fused_object.class_, (int(fused_object.bbox2d[0]+30),int(fused_object.bbox2d[1]+30)), cv2.FONT_HERSHEY_SIMPLEX, 0.75, (200, 200, 100), 3, cv2.LINE_AA)    \n",
        "    return final_image, list_of_fused_objects\n"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "D7A3DLKQeMrk"
      },
      "source": [
        "final_image, _ = build_fused_object(list_of_2d_objects, list_of_3d_objects, matches, lidar_2d)\n",
        "\n",
        "plt.figure(figsize=(14,7))\n",
        "plt.imshow(final_image)\n",
        "plt.show()"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "HFrHfqCPJbaN"
      },
      "source": [
        "## Create the Complete Pipeline"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "mwiqCaHDD7aX"
      },
      "source": [
        "def pipeline(img, calib_file):\n",
        "    lidar2cam = LiDAR2Camera(calib_file)\n",
        "    # 1 - Run Obstacle Detection\n",
        "    result, pred_bboxes = run_obstacle_detection(image)\n",
        "\n",
        "    # 2 - Build a 2D Object\n",
        "    list_of_2d_objects = fill_2D_obstacles(result, pred_bboxes)\n",
        "    \n",
        "    # 3 - Build a 3D Object (from labels)\n",
        "    list_of_3d_objects = read_label(label_files[index])\n",
        "    \n",
        "    # 4 - Get the LiDAR Boxes in the Image in 2D and 3D\n",
        "    lidar_2d, lidar_3d =lidar2cam.get_image_with_bboxes(image, list_of_3d_objects)\n",
        "    \n",
        "    # 5 - Associate the LiDAR boxes and the Camera Boxes\n",
        "    lidar_boxes = [obs.bbox2d for obs in list_of_3d_objects] # Simply get the boxes\n",
        "    camera_boxes = [obs.bbox for obs in list_of_2d_objects]\n",
        "    matches = associate(lidar_boxes, camera_boxes)\n",
        "    \n",
        "    #6 - Build a Fused Object\n",
        "    final_image, _ = build_fused_object(list_of_2d_objects, list_of_3d_objects, matches, lidar_2d)\n",
        "    \n",
        "    return final_image\n"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "KMm_b22hLFzU"
      },
      "source": [
        "image_files = sorted(glob.glob(\"data/img/*.png\"))\n",
        "label_files = sorted(glob.glob(\"data/label/*.txt\"))\n",
        "calib_files = sorted(glob.glob(\"data/calib/*.txt\"))\n",
        "\n",
        "print(\"There are\",len(image_files),\"images\")\n",
        "index = 1\n",
        "image = cv2.cvtColor(cv2.imread(image_files[index]), cv2.COLOR_BGR2RGB)\n",
        "plt.figure(figsize=(14,7))\n",
        "plt.imshow(pipeline(image, calib_files[index]))\n",
        "plt.show()"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "vjYC0cbihf8R"
      },
      "source": [
        ""
      ],
      "execution_count": null,
      "outputs": []
    }
  ]
}