Project_a_3D_Point_to_a_2D_Image.ipynb

{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "colab": {
      "name": "Project a 3D Point to a 2D Image.ipynb",
      "private_outputs": true,
      "provenance": [],
      "collapsed_sections": [
        "1rbhQKptg-uK"
      ],
      "authorship_tag": "ABX9TyMGhb1JvYuQXUK4IiXDCHRQ",
      "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/Project_a_3D_Point_to_a_2D_Image.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 LiDAR Camera Projection Workshop!\n",
        "\n",
        "We are going to learn:\n",
        "1. How to **Load a Point Cloud** and **Visualize it**\n",
        "2. How to **Load an Image** and **Visualize it**\n",
        "3. How to **fuse LiDAR and Camera manually** (i.e. without coding it)\n",
        "4. How to **project LiDAR Point Clouds to an Image**\n",
        "5. How to **run the algorithms on a video**\n",
        "\n",
        "Ready? Let's get started!"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "XGlg7uxki1Cq"
      },
      "source": [
        "## Load the Data and Visualize it!"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "-RYckDyji8H1"
      },
      "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": "vrs7OD_Vi_Zs"
      },
      "source": [
        "### Import the necessary libraries"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "pppZPZ3_jBKN"
      },
      "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",
        "import glob\n",
        "import open3d as o3d"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "wr79FMSMpT8E"
      },
      "source": [
        "### Load the Files"
      ]
    },
    {
      "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",
        "index = 0\n",
        "pcd_file = point_files[index]\n",
        "image = cv2.cvtColor(cv2.imread(image_files[index]), cv2.COLOR_BGR2RGB)\n",
        "cloud = o3d.io.read_point_cloud(pcd_file)\n",
        "points= np.asarray(cloud.points)"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "1rbhQKptg-uK"
      },
      "source": [
        "### Optional - If your LiDAR file is in binary extension '.bin', use this piece of code to turn it into a '.pcd' and save it"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "Zq7lPkvYPXow"
      },
      "source": [
        "## BIN TO PCD\n",
        "import numpy as np\n",
        "import struct\n",
        "size_float = 4\n",
        "list_pcd = []\n",
        "\n",
        "file_to_open = point_files[index]\n",
        "file_to_save = str(point_files[index])[:-3]+\"pcd\"\n",
        "with open (file_to_open, \"rb\") as f:\n",
        "    byte = f.read(size_float*4)\n",
        "    while byte:\n",
        "        x,y,z,intensity = struct.unpack(\"ffff\", byte)\n",
        "        list_pcd.append([x, y, z])\n",
        "        byte = f.read(size_float*4)\n",
        "np_pcd = np.asarray(list_pcd)\n",
        "pcd = o3d.geometry.PointCloud()\n",
        "v3d = o3d.utility.Vector3dVector\n",
        "pcd.points = v3d(np_pcd)\n",
        "\n",
        "o3d.io.write_point_cloud(file_to_save, pcd)\n"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "00YaQZjrjRpW"
      },
      "source": [
        "### Visualize the Image"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "YnSsfviYjTxb"
      },
      "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": "tiNl7OdgjUri"
      },
      "source": [
        "### Visualize the Point Clouds"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "VFRpDWNXjazV"
      },
      "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": "44-hPZXmTulS"
      },
      "source": [
        "##Project the Points in the Image <p>\n",
        "That part is possibly the hardest to understand and will require your full attention. We want to project the 3D points into the image.<p>\n",
        "\n",
        "It means we'll need to: <p>\n",
        "\n",
        "*   Select the Point that are **visible** in the image 🤔\n",
        "*   Convert the Points **from the LiDAR frame to the Camera Frame** 🤯\n",
        "*   Find a way to project **from the Camera Frame to the Image Frame** 😭\n",
        "\n",
        "<p>\n",
        "No worries here, we'll figure out everything together.\n"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "aTqtmQJoBx-8"
      },
      "source": [
        "### 1 - Read the Calibration File\n",
        "\n",
        "The first step is to read the calibration files. For each image, we have an associated calibration file that states:<p>\n",
        "\n",
        "\n",
        "*   The instrinsic and extrinsic camera calibration parameters\n",
        "*   The velodyne to camera matrices\n",
        "*   All the other \"sensor A\" to \"sensor B\" matrices\n",
        "<p>\n",
        "They are made from this setup:<p>\n",
        "\n",
        "![link text](http://www.cvlibs.net/datasets/kitti/images/setup_top_view.png)\n",
        "\n",
        "Not everything matters to us here, only a few things:\n",
        "*    **Velo-To-Cam is a variable we'll call V2C** -- It gives the rotation and translation matrices from the Velodyne to the Left Grayscale camera.\n",
        "*    **R0_rect is used in Stereo Vision** to make the images co-planar. It's our case here.\n",
        "*   **P2 is the matrix obtained after camera calibration**. It contains the intrinsic matrix K and the extrinsic.\n"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "fSzz_3tf0xlX"
      },
      "source": [
        "class LiDAR2Camera(object):\n",
        "    def __init__(self, calib_file):\n",
        "        calibs = self.read_calib_file(calib_file)\n",
        "        P = calibs[\"P2\"]\n",
        "        self.P = np.reshape(P, [3, 4])\n",
        "        # Rigid transform from Velodyne coord to reference camera coord\n",
        "        V2C = calibs[\"Tr_velo_to_cam\"]\n",
        "        self.V2C = np.reshape(V2C, [3, 4])\n",
        "        # Rotation from reference camera coord to rect camera coord\n",
        "        R0 = calibs[\"R0_rect\"]\n",
        "        self.R0 = np.reshape(R0, [3, 3])\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": "code",
      "metadata": {
        "id": "aZWuxKSFFEDA"
      },
      "source": [
        "lidar2cam = LiDAR2Camera(calib_files[index])\n",
        "print(\"P :\"+str(lidar2cam.P))\n",
        "print(\"-\")\n",
        "print(\"RO \"+str(lidar2cam.R0))\n",
        "print(\"-\")\n",
        "print(\"Velo 2 Cam \" +str(lidar2cam.V2C))\n",
        "print(\"-\")\n",
        "#print(\"Cam 2 Velo\" + str(lidar2cam.C2V))"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "zZ4qEcG9CNJq"
      },
      "source": [
        "### 2 - Apply the Magic Formula\n",
        "\n",
        "The main formula we'll use will be as follows:<p>\n",
        "**Y(2D) = P x R0 x R|t x X (3D)** \n",
        "\n",
        "However, when looking at the dimensions:\n",
        "\n",
        "*   P: [3x4]\n",
        "*   R0: [3x3]\n",
        "*   R|t = Velo2Cam: [3x4]\n",
        "*   X: [3x1]\n",
        "\n",
        "We'll need to convert some points into Homogeneous Coordinates:\n",
        "* RO must go from 3x3 to 4x3\n",
        "* x must go from 3x1 to 4x1\n",
        "\n",
        "Then, to retrieve the cartesian system, we'll divide as explained in the course."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "-jB2aXeITxS8"
      },
      "source": [
        "def cart2hom(self, pts_3d):\n",
        "    \"\"\" Input: nx3 points in Cartesian\n",
        "        Oupput: nx4 points in Homogeneous by pending 1\n",
        "    \"\"\"\n",
        "    n = pts_3d.shape[0]\n",
        "    pts_3d_hom = np.hstack((pts_3d, np.ones((n, 1))))\n",
        "    return pts_3d_hom\n",
        "\n",
        "LiDAR2Camera.cart2hom = cart2hom"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "siG14wbNcVCK"
      },
      "source": [
        "def project_velo_to_image(self, pts_3d_velo):\n",
        "    '''\n",
        "    Input: 3D points in Velodyne Frame [nx3]\n",
        "    Output: 2D Pixels in Image Frame [nx2]\n",
        "    '''\n",
        "    # REVERSE TECHNIQUE\n",
        "    '''\n",
        "    homogeneous = self.cart2hom(pts_3d_velo)  # nx4\n",
        "    dotted_RT = np.dot(homogeneous, np.transpose(self.V2C)) #nx3\n",
        "    dotted_with_RO = np.transpose(np.dot(self.R0, np.transpose(dotted_RT))) #nx3\n",
        "    homogeneous_2 = self.cart2hom(dotted_with_RO) #nx4\n",
        "    pts_2d = np.dot(homogeneous_2, np.transpose(self.P))  # nx3\n",
        "    '''\n",
        "    \n",
        "    # NORMAL TECHNIQUE\n",
        "    R0_homo = np.vstack([self.R0, [0, 0, 0]])\n",
        "    R0_homo_2 = np.column_stack([R0_homo, [0, 0, 0, 1]])\n",
        "    p_r0 = np.dot(self.P, R0_homo_2) #PxR0\n",
        "    p_r0_rt =  np.dot(p_r0, np.vstack((self.V2C, [0, 0, 0, 1]))) #PxROxRT\n",
        "    pts_3d_homo = np.column_stack([pts_3d_velo, np.ones((pts_3d_velo.shape[0],1))])\n",
        "    p_r0_rt_x = np.dot(p_r0_rt, np.transpose(pts_3d_homo))#PxROxRTxX\n",
        "    pts_2d = np.transpose(p_r0_rt_x)\n",
        "    \n",
        "    pts_2d[:, 0] /= pts_2d[:, 2]\n",
        "    pts_2d[:, 1] /= pts_2d[:, 2]\n",
        "    return pts_2d[:, 0:2]\n",
        "\n",
        "LiDAR2Camera.project_velo_to_image = project_velo_to_image\n",
        "print(points[:1,:3])\n",
        "print(\"Euclidean Pixels \"+str(lidar2cam.project_velo_to_image(points[:1,:3])))\n"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "F0NSdCLpCgBP"
      },
      "source": [
        "### 1.3 - Get the Points in the Field Of View Only"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "cICtSHfD8I8Z"
      },
      "source": [
        "def get_lidar_in_image_fov(self,pc_velo, xmin, ymin, xmax, ymax, return_more=False, clip_distance=2.0):\n",
        "    \"\"\" Filter lidar points, keep those in image FOV \"\"\"\n",
        "    pts_2d = self.project_velo_to_image(pc_velo)\n",
        "    fov_inds = (\n",
        "        (pts_2d[:, 0] < xmax)\n",
        "        & (pts_2d[:, 0] >= xmin)\n",
        "        & (pts_2d[:, 1] < ymax)\n",
        "        & (pts_2d[:, 1] >= ymin)\n",
        "    )\n",
        "    fov_inds = fov_inds & (pc_velo[:, 0] > clip_distance) # We don't want things that are closer to the clip distance (2m)\n",
        "    imgfov_pc_velo = pc_velo[fov_inds, :]\n",
        "    if return_more:\n",
        "        return imgfov_pc_velo, pts_2d, fov_inds\n",
        "    else:\n",
        "        return imgfov_pc_velo\n",
        "    \n",
        "LiDAR2Camera.get_lidar_in_image_fov = get_lidar_in_image_fov"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "UvBO5HB6m_22"
      },
      "source": [
        "lidar2cam.get_lidar_in_image_fov(points[:1,:], 0, 0, img.shape[1], img.shape[0], True)\n"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "hVNFq-3gCnmL"
      },
      "source": [
        "###1.4 -- Draw the Points in the Image"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "1DOf4AXe8Kqf"
      },
      "source": [
        "def show_lidar_on_image(self, pc_velo, img, debug=\"False\"):\n",
        "    \"\"\" Project LiDAR points to image \"\"\"\n",
        "    imgfov_pc_velo, pts_2d, fov_inds = self.get_lidar_in_image_fov(\n",
        "        pc_velo, 0, 0, img.shape[1], img.shape[0], True\n",
        "    )\n",
        "    if (debug==True):\n",
        "        print(\"3D PC Velo \"+ str(imgfov_pc_velo)) # The 3D point Cloud Coordinates\n",
        "        print(\"2D PIXEL: \" + str(pts_2d)) # The 2D Pixels\n",
        "        print(\"FOV : \"+str(fov_inds)) # Whether the Pixel is in the image or not\n",
        "    self.imgfov_pts_2d = pts_2d[fov_inds, :]\n",
        "    '''\n",
        "    #homogeneous = np.hstack((imgfov_pc_velo, np.ones((imgfov_pc_velo.shape[0], 1))))\n",
        "    homogeneous = self.cart2hom(imgfov_pc_velo)\n",
        "    transposed_RT = np.dot(homogeneous, np.transpose(self.V2C))\n",
        "    dotted_RO = np.transpose(np.dot(self.R0, np.transpose(transposed_RT)))\n",
        "    self.imgfov_pc_rect = dotted_RO\n",
        "    \n",
        "    if debug==True:\n",
        "        print(\"FOV PC Rect \"+ str(self.imgfov_pc_rect))\n",
        "    '''\n",
        "    cmap = plt.cm.get_cmap(\"hsv\", 256)\n",
        "    cmap = np.array([cmap(i) for i in range(256)])[:, :3] * 255\n",
        "    self.imgfov_pc_velo = imgfov_pc_velo\n",
        "    \n",
        "    for i in range(self.imgfov_pts_2d.shape[0]):\n",
        "        #depth = self.imgfov_pc_rect[i,2]\n",
        "        #print(depth)\n",
        "        depth = imgfov_pc_velo[i,0]\n",
        "        #print(depth)\n",
        "        color = cmap[int( 510.0 / depth), :]\n",
        "        cv2.circle(\n",
        "            img,(int(np.round(self.imgfov_pts_2d[i, 0])), int(np.round(self.imgfov_pts_2d[i, 1]))),10,\n",
        "            color=tuple(color),\n",
        "            thickness=-1,\n",
        "        )\n",
        "\n",
        "    return img\n",
        "\n",
        "LiDAR2Camera.show_lidar_on_image = show_lidar_on_image"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "AaLfy002oOIG"
      },
      "source": [
        "#img_3 = lidar2cam.show_lidar_on_image(points[:,:3], image)\n",
        "img_3 = image.copy()\n",
        "img_3 = lidar2cam.show_lidar_on_image(points[:,:3], img_3)\n",
        "plt.figure(figsize=(14,7))\n",
        "plt.imshow(img_3)\n",
        "plt.show()"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "6eZq-Put27fl"
      },
      "source": [
        "## Shooting a Portfolio Video"
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "VieqzsWS2-Wk"
      },
      "source": [
        "video_images = sorted(glob.glob(\"data/video/images/*.png\"))\n",
        "video_points = sorted(glob.glob(\"data/video/points/*.pcd\"))\n",
        "\n",
        "# Build a LiDAR2Cam object\n",
        "lidar2cam_video = LiDAR2Camera(calib_files[0])\n",
        "\n",
        "result_video = []\n",
        "\n",
        "for idx, img in enumerate(video_images):\n",
        "    image = cv2.cvtColor(cv2.imread(img), cv2.COLOR_BGR2RGB)\n",
        "    point_cloud = np.asarray(o3d.io.read_point_cloud(video_points[idx]).points)\n",
        "    result_video.append(lidar2cam_video.show_lidar_on_image(point_cloud, image))\n",
        "\n",
        "out = cv2.VideoWriter('output/out.avi',cv2.VideoWriter_fourcc(*'DIVX'), 15, (image.shape[1],image.shape[0]))\n",
        " \n",
        "for i in range(len(result_video)):\n",
        "    out.write(cv2.cvtColor(result_video[i], cv2.COLOR_BGR2RGB))\n",
        "    #out.write(result_video[i])\n",
        "out.release()"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "rPv8-awDoNL_"
      },
      "source": [
        ""
      ],
      "execution_count": null,
      "outputs": []
    }
  ]
}