KITTI_Mapping_Tutorial_Step_by_step.ipynb

{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "colab_type": "text",
        "id": "view-in-github"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/MukhlasAdib/KITTI_Mapping/blob/main/KITTI_Mapping_Tutorial_Step_by_step.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "vYlYRBayIP1p"
      },
      "source": [
        "# REFERENCES\n",
        "\n",
        "[1] Repository for this tutorial: https://github.com/MukhlasAdib/KITTI_Mapping.\n",
        "\n",
        "[2] The full KITTI datased can be accessed here: http://www.cvlibs.net/datasets/kitti/.\n",
        "\n",
        "[3] KITTI Dataset paper: A. Geiger, P. Lenz, C. Stiller and R. Urtasun, \"Vision meets Robotics: The KITTI Dataset,\" *International Journal of Robotics Research (IJRR)*, vol. 32, no. 11, pp. 1231-1237 2013.\n",
        "\n",
        "[4] Description of Occupancy Grid Map (OGM) estimation: Z. Luo, M. V. Mohrenschilt and S. Habibi, \"A probability occupancy grid based approach for real-time LiDAR ground segmentation,\" *IEEE Transactions on Intelligent Transportation Systems*, vol 21, no. 3, pp. 998–1010, Mar. 2020.\n",
        "\n",
        "[5] Description of Dynamic Grid Map (DGM) estimation: J. Moras, V. Cherfaoui and P. Bonnifait, \"Credibilist occupancy grids for vehicle perception in dynamic environments,\" *2011 IEEE International Conference on Robotics and Automation*, Shanghai, China, 2011, pp. 84-89.\n",
        "\n",
        "[6] Paper of DeepLab v3+ for image segmentation: L. C. Chen, Y. Zhu, G. Apandreou, F. Schroff and H. Adam, “Encoder-decoder with atrous separable convolution for semantic image segmentation,” *ECCV 2018 Lecture Notes in Computer Science*, vol. 11211, pp. 833–851, 2018.\n",
        "\n",
        "[7] DeepLab v3+ paper via arXiv: https://arxiv.org/abs/1802.02611.\n",
        "\n",
        "[8] DeepLab v3+ repository: https://github.com/tensorflow/models/tree/master/research/deeplab.\n",
        "\n",
        "[9] This tutorial use pykitti module to load the KITTI dataset: https://github.com/utiasSTARS/pykitti."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "iderI-dPJ7QJ"
      },
      "source": [
        "# PREPARATION"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "4lDKfTLg5WQo"
      },
      "outputs": [],
      "source": [
        "!git clone https://github.com/MukhlasAdib/KITTI_Mapping.git\n",
        "!pip install pykitti\n",
        "!pip install opencv-python==3.4.18.65 scipy==1.7.3"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "FBjbCBkk6gI1"
      },
      "outputs": [],
      "source": [
        "import os\n",
        "import sys\n",
        "import numpy as np\n",
        "import cv2\n",
        "import matplotlib.pyplot as plt\n",
        "import pykitti\n",
        "import tensorflow as tf\n",
        "from sklearn.linear_model import RANSACRegressor\n",
        "from scipy import stats"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "SC0LVdNh66r6"
      },
      "outputs": [],
      "source": [
        "### Load KITTI Data\n",
        "basedir = 'KITTI_Mapping/raw_data/'\n",
        "date = '2011_09_26'\n",
        "drive = '0013'\n",
        "data = pykitti.raw(basedir, date, drive)\n",
        "\n",
        "### Index of data used for test\n",
        "### We use two data from different time\n",
        "idx = 50\n",
        "idx1 = idx+10"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "cDiDJTa7J-fB"
      },
      "source": [
        "# PERCEPTION\n",
        "\n",
        "The goal of the perception system is to extract the information about the round where the vehicle is operating on. This road information will be used to filter out LiDAR points that hit the road so that it can be used for mapping purpose. To extract the information about where the road is, we use deep learning-based image segmentation technique that will be applied to the camera image."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "uVQYbI6wAbdv"
      },
      "source": [
        "## Get Callibration Data"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "UoBXqa6l2u8a"
      },
      "source": [
        "Two callibrated parameters that we need:\n",
        "- LiDAR to camera extrinsic matrix - The matrix (4x4) that will be used to transform the LiDAR points to the camera 3D coordinate frame.\n",
        "-  Camera intrinsic matrix - The mastrix (3x3) that will be used to calculate the coordinate of pixels that representat 3D points in camera coordinate.\n",
        "\n",
        "The calibrated parameters are already provided in the dataset. For details, please take a look at [this explanation](https://www.mathworks.com/help/vision/ug/camera-calibration.html) from Mathworks."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "-24HbwND-fbH"
      },
      "outputs": [],
      "source": [
        "### Retrieve the provided calibration data\n",
        "lidar2cam_extrinsic = data.calib.T_cam2_velo\n",
        "camera_intrinsic = data.calib.K_cam2\n",
        "\n",
        "print('Lidar to camera extrinsic matrix: ')\n",
        "print(lidar2cam_extrinsic)\n",
        "print()\n",
        "print('Camera intrinsic matrix: ')\n",
        "print(camera_intrinsic)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "y2sVVXDiAfw5"
      },
      "source": [
        "## Load Camera and LiDAR Data"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "TTycCXXRACXM"
      },
      "source": [
        "Coordinate system of\n",
        "- Camera = x: right, y: down, z: forward\n",
        "- LiDAR = x: forward, y: left, z: up\n",
        "\n",
        "In this tutorial, camera coordinate system is used. Therefore, the LiDAR points need to be transformed to the camera coordinate frame. Use the LiDAR to camera extrinsic matrix!\n",
        "\n",
        "The extrinsic matrix can be written as $\\begin{bmatrix} R|t \\end{bmatrix}$, a combinarion of a rotation matrix $R$ and a translation vector $t$. Given that the point in LiDAR coordinate is $(X_L,Y_L,Z_L)$, its coordinate in camera 3D frame, $(X_C,Y_C,Z_C)$ is\n",
        "\n",
        "\\begin{align}\n",
        "\\begin{bmatrix}\n",
        "X_C \\\\\n",
        "Y_C \\\\\n",
        "Z_C \\\\\n",
        "1 \\\\\n",
        "\\end{bmatrix}= \n",
        "\\begin{bmatrix} R|t \\end{bmatrix}\n",
        "\\begin{bmatrix}\n",
        "X_L \\\\\n",
        "Y_L \\\\\n",
        "Z_L \\\\\n",
        "1 \\\\\n",
        "\\end{bmatrix}\n",
        "\\end{align}\n",
        "\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "kacf9ugO_XGU"
      },
      "outputs": [],
      "source": [
        "def load_data(data,idx):\n",
        "  ### Get the image data\n",
        "  img_raw = np.array(data.get_cam2(idx))\n",
        "\n",
        "  ### Get the LiDAR data (only x,y,z data)\n",
        "  ### Only use LiDAR points that are below the sensor\n",
        "  ### Only use LiDAR points that are at least 2.5 m away\n",
        "  lidar_raw = np.array(data.get_velo(idx))[:,:3]\n",
        "  lidar_raw = lidar_raw[lidar_raw[:,2]<=0,:]\n",
        "  dist = np.linalg.norm(lidar_raw,axis=1)\n",
        "  lidar_raw = lidar_raw[dist >= 2.5]\n",
        "  return img_raw,lidar_raw\n",
        "\n",
        "### Transform the LiDAR points into camera coordinate\n",
        "def transform_coordinate(lidar_points,extrinsic_matrix):\n",
        "  inp = lidar_points.copy()\n",
        "  inp = np.concatenate((inp,np.ones((inp.shape[0],1))),axis=1)\n",
        "  inp = np.matmul(extrinsic_matrix,inp.T).T\n",
        "  return inp[:,:3]\n",
        "\n",
        "img_raw,lidar_raw = load_data(data,idx)\n",
        "img_raw_size = img_raw.shape\n",
        "lidar_raw = transform_coordinate(lidar_raw,lidar2cam_extrinsic)\n",
        "\n",
        "img_raw1,lidar_raw1 = load_data(data,idx1)\n",
        "img_raw1_size = img_raw1.shape\n",
        "lidar_raw1 = transform_coordinate(lidar_raw1,lidar2cam_extrinsic)\n",
        "\n",
        "### Visualize\n",
        "fig,axs = plt.subplots(2,2,figsize=(15,12))\n",
        "axs[0,0].imshow(img_raw)\n",
        "axs[0,1].scatter(lidar_raw[:,0],lidar_raw[:,2],c=-lidar_raw[:,1],marker='.')\n",
        "axs[0,1].scatter(0,0,c='r',marker='x')\n",
        "axs[0,1].axis('scaled')\n",
        "axs[1,0].imshow(img_raw1)\n",
        "axs[1,1].scatter(lidar_raw1[:,0],lidar_raw1[:,2],c=-lidar_raw1[:,1],marker='.')\n",
        "axs[1,1].scatter(0,0,c='r',marker='x')\n",
        "axs[1,1].axis('scaled')\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "3QTNtHYfCs2e"
      },
      "source": [
        "## Project the LiDAR Points to the Camera"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Ytg6gknC6nYY"
      },
      "source": [
        "To transfer the road information from image to LiDAR, we need to project the LiDAR points to the camera image. Use the camera intrinsic matrix!\n",
        "\n",
        "The camera intrinsic matrix is denoted as $K$. The coordinate of pixel, $(u,v)$, that represents a point in 3D space, $(X_C,Y_C,Z_C)$, in image frame can be calculated with \n",
        "\n",
        "\\begin{align}\n",
        "\\begin{bmatrix}\n",
        "u\\times w \\\\\n",
        "v\\times w \\\\\n",
        "w \\\\\n",
        "\\end{bmatrix}= \n",
        "K\n",
        "\\begin{bmatrix}\n",
        "X_C \\\\\n",
        "Y_C \\\\\n",
        "Z_C \\\\\n",
        "\\end{bmatrix}\n",
        "\\end{align}\n",
        "\n",
        "<br>\n",
        "<center><img src=\"https://docs.opencv.org/4.5.0/pinhole_camera_model.png\" width=400px></center>\n",
        "</br>\n",
        "\n",
        "Sumber gambar: [OpenCV Docs](https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "uwSf1GLfCr1T"
      },
      "outputs": [],
      "source": [
        "def project_lidar2cam(lidar_in_cam,camera_intrinsic,img_raw_size):\n",
        "  ### Filter out data behind the cam\n",
        "  lidar_in_cam = np.concatenate((lidar_in_cam,np.ones((lidar_in_cam.shape[0],1))),axis=1)\n",
        "  lidar_in_cam = lidar_in_cam[lidar_in_cam[:,2]>0]\n",
        "\n",
        "  ### Project points to the image\n",
        "  lidar_2d = np.matmul(camera_intrinsic,lidar_in_cam[:,:3].T).T\n",
        "  lidar_2d = np.divide(lidar_2d,lidar_2d[:,2].reshape((-1,1)))\n",
        "  lidar_2d = lidar_2d.astype(int)\n",
        "\n",
        "  ### Filter out points that are outside image frame\n",
        "  maskH = np.logical_and(lidar_2d[:,0]>=0,lidar_2d[:,0]<img_raw_size[1])\n",
        "  maskV = np.logical_and(lidar_2d[:,1]>=0,lidar_2d[:,1]<img_raw_size[0])\n",
        "  mask = np.logical_and(maskH,maskV)\n",
        "  lidar_2d = lidar_2d[mask,:]\n",
        "  lidar_in_cam = lidar_in_cam[mask,:]\n",
        "\n",
        "  return lidar_2d,lidar_in_cam[:,:3]\n",
        "\n",
        "lidar_2d,lidar_in_cam = project_lidar2cam(lidar_raw,camera_intrinsic,img_raw_size)\n",
        "lidar_2d1,lidar_in_cam1 = project_lidar2cam(lidar_raw1,camera_intrinsic,img_raw_size)\n",
        "\n",
        "### Visualize\n",
        "print(f'Original image size: {img_raw.shape}')\n",
        "\n",
        "img = img_raw.copy()\n",
        "axs = 2\n",
        "axs_log = np.log(lidar_in_cam[:,axs]-np.min(lidar_in_cam[:,axs])+1)\n",
        "max_axs = np.max(axs_log)\n",
        "for pt,z in zip(lidar_2d,axs_log):\n",
        "    color_z = z*255/max_axs\n",
        "    c = (color_z,0,0)\n",
        "    cv2.circle(img,tuple(pt[:2].astype(int)),1,c,-1)\n",
        "\n",
        "img1 = img_raw1.copy()\n",
        "axs = 2\n",
        "axs_log1 = np.log(lidar_in_cam1[:,axs]-np.min(lidar_in_cam1[:,axs])+1)\n",
        "max_axs1 = np.max(axs_log1)\n",
        "for pt,z in zip(lidar_2d1,axs_log1):\n",
        "    color_z = z*255/max_axs1\n",
        "    c = (color_z,0,0)\n",
        "    cv2.circle(img1,tuple(pt[:2].astype(int)),1,c,-1)\n",
        "\n",
        "fig,axs = plt.subplots(2,1,figsize=(15,7))\n",
        "axs[0].imshow(img)\n",
        "axs[1].imshow(img1)\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "1aTnCvzTGGO-"
      },
      "source": [
        "## Image Cropping"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Ya3jzuZL7GzM"
      },
      "source": [
        "The deep learning model (DeepLab v3+) was trained with cropped images from KITTI dataset with ratio 4:3 (W:H), which was resized further to 513 x 513 images. It will work better if we use the same size of images.\n",
        "\n",
        "Parameter:\n",
        "- CROP_RH - The height ratio of target image size\n",
        "- CROP_RW - The width ratio of target image size"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "r7aD86U97LyP"
      },
      "outputs": [],
      "source": [
        "### Function to crop the image according to the target size ratio\n",
        "### The image will be cropped at the center\n",
        "def crop_data(img_in,lidar_2d_in,lidar_in_cam_in,rh,rw):\n",
        "  lidar_2d = lidar_2d_in.copy()\n",
        "  lidar_in_cam = lidar_in_cam_in.copy()\n",
        "  img = img_in.copy()\n",
        "\n",
        "  ### Crop the image\n",
        "  dim_ori = np.array(img.shape)\n",
        "  cent = (dim_ori/2).astype(int)\n",
        "  if dim_ori[0]/dim_ori[1] == rh/rw:\n",
        "      crop_img = img\n",
        "    \n",
        "  # If Height <= Width\n",
        "  elif dim_ori[0] <= dim_ori[1]:\n",
        "      cH2 = dim_ori[0]\n",
        "      cW2 = cH2*rw/rh\n",
        "      cW = int(cW2/2)\n",
        "      crop_img = img[:,cent[1]-cW:cent[1]+cW+1]\n",
        "\n",
        "  # If Height > Width\n",
        "  else:\n",
        "      cW2 = dim_ori[1]\n",
        "      cH2 = cW2*rh/rw\n",
        "      cH = int(cH2/2)\n",
        "      crop_img = img[cent[0]-cH:cent[0]+cH+1,:]\n",
        "\n",
        "  ### Filter out LiDAR points outside cropped image\n",
        "  cW = cW2/2\n",
        "  cH = cH2/2\n",
        "  centH = cent[0]\n",
        "  centW = cent[1]\n",
        "  maskH = np.logical_and(lidar_2d[:,1]>=centH-cH,lidar_2d[:,1]<=centH+cH)\n",
        "  maskW = np.logical_and(lidar_2d[:,0]>=centW-cW,lidar_2d[:,0]<=centW+cW)\n",
        "  mask = np.logical_and(maskH,maskW)\n",
        "  lidar_2d = lidar_2d[mask,:]\n",
        "  lidar_in_cam = lidar_in_cam[mask,:]\n",
        "  cent = np.array((centW-cW,centH-cH,0)).reshape((1,3))\n",
        "  lidar_2d = lidar_2d - cent\n",
        "\n",
        "  return crop_img, lidar_2d.astype(int), lidar_in_cam\n",
        "\n",
        "### Cropped image's size ratio\n",
        "CROP_RH = 3 # Height ratio\n",
        "CROP_RW = 4 # Width ratio\n",
        "crop_img,lidar_2d,lidar_in_cam = crop_data(img_raw,lidar_2d,lidar_in_cam,CROP_RH,CROP_RW)\n",
        "crop_img1,lidar_2d1,lidar_in_cam1 = crop_data(img_raw1,lidar_2d1,lidar_in_cam1,CROP_RH,CROP_RW)\n",
        "\n",
        "### Visualize\n",
        "img = crop_img.copy()\n",
        "axs = 2\n",
        "axs_log = np.log(lidar_in_cam[:,axs]-np.min(lidar_in_cam[:,axs])+1)\n",
        "max_axs = np.max(axs_log)\n",
        "for pt,z in zip(lidar_2d,axs_log):\n",
        "    color_z = z*255/max_axs\n",
        "    c = (color_z,0,0)\n",
        "    cv2.circle(img,tuple(pt[:2].astype(int)),1,c,-1)\n",
        "\n",
        "img1 = crop_img1.copy()\n",
        "axs = 2\n",
        "axs_log1 = np.log(lidar_in_cam1[:,axs]-np.min(lidar_in_cam1[:,axs])+1)\n",
        "max_axs1 = np.max(axs_log1)\n",
        "for pt,z in zip(lidar_2d1,axs_log1):\n",
        "    color_z = z*255/max_axs1\n",
        "    c = (color_z,0,0)\n",
        "    cv2.circle(img1,tuple(pt[:2].astype(int)),1,c,-1)\n",
        "\n",
        "fig,axs = plt.subplots(1,2,figsize=(10,7))\n",
        "axs[0].imshow(img)\n",
        "axs[1].imshow(img1)\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "zhy-xR-xKPu6"
      },
      "source": [
        "## Road Segmentation"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "mxb-TFz0ATMJ"
      },
      "source": [
        "Process the camera image with image segmentation-based deep learning method to get regions that correspond to the road. Specifically, we use DeepLab v3+ model that has been trained before with KITTI dataset. The model has been trained on image with size 513 x 513. Thus, the input image need to be resized first before being processed by the model. [Read this article](https://rockyshikoku.medium.com/train-deeplab-v3-with-your-own-dataset-13f2af958a75) if you want to know how to train your own DeepLab v3+ model. \n",
        "\n",
        "Parameters:\n",
        "- DEEPLAB_MODEL_PATH - Path to the model protobuff (.pb) file\n",
        "- DEEPLAB_INPUT_SIZE - Input size of the model\n",
        "\n",
        "Now, by projecting the LiDAR points to the segmented image, we can know which points that correspond to the road. "
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "vM6rRV1h733l"
      },
      "outputs": [],
      "source": [
        "### Function to process the image with DeepLabv3+\n",
        "def process_images(img_in, sess, target_size=513, probability_threshold=0.5):\n",
        "  INPUT_TENSOR_NAME = 'ImageTensor:0'\n",
        "  PROB_TENSOR_NAME = 'SemanticProbabilities:0'\n",
        "  INPUT_SIZE = target_size\n",
        "\n",
        "  image = img_in.copy()\n",
        "  sz = image.shape\n",
        "  image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)\n",
        "\n",
        "  # Resize input image to target size\n",
        "  if INPUT_SIZE == 0:\n",
        "    resized_image = image.copy()\n",
        "  else:\n",
        "    resized_image = cv2.resize(image,(INPUT_SIZE,INPUT_SIZE))\n",
        "\n",
        "  # Run deep learning inference\n",
        "  batch_seg_map = sess.run(\n",
        "      PROB_TENSOR_NAME,\n",
        "      feed_dict={INPUT_TENSOR_NAME: [np.asarray(resized_image)]})\n",
        "  seg_map = (batch_seg_map[0][:,:,1]*255).astype(int)\n",
        "  prob = np.array(seg_map, dtype=np.uint8)\n",
        "  prob = cv2.resize(prob,(sz[1],sz[0]))\n",
        "\n",
        "  # Create the prediction\n",
        "  pred = prob.copy()\n",
        "  msk_bin = prob >= (probability_threshold*255)\n",
        "  pred[msk_bin] = 1\n",
        "  pred[np.logical_not(msk_bin)] = 0\n",
        "\n",
        "  # Ignore regions that are separated from the main road\n",
        "  # This can reduce the amount of false detection\n",
        "  _,segm_reg = cv2.connectedComponents(pred)\n",
        "  segm_reg = segm_reg.astype(float)\n",
        "  segm_reg[segm_reg==0] = np.nan\n",
        "  modes,_ = stats.mode(segm_reg.flatten(),axis=None,nan_policy=\"omit\")\n",
        "  mode = modes[0]\n",
        "  pred[segm_reg!=mode] = 0\n",
        "  \n",
        "  return prob,(pred*255).astype(np.uint8)\n",
        "\n",
        "### Load the model\n",
        "DEEPLAB_MODEL_PATH = 'KITTI_Mapping/pretrained/deeplab_model.pb'\n",
        "with open(DEEPLAB_MODEL_PATH, \"rb\") as f:\n",
        "    graph_def = tf.compat.v1.GraphDef.FromString(f.read())\n",
        "graph = tf.Graph()\n",
        "with graph.as_default():\n",
        "    tf.import_graph_def(graph_def=graph_def, name=\"\")\n",
        "sess = tf.compat.v1.Session(graph=graph)\n",
        "\n",
        "DEEPLAB_INPUT_SIZE = 513\n",
        "segm_prob,segm_pred = process_images(crop_img, sess, DEEPLAB_INPUT_SIZE, 0.5)\n",
        "segm_prob1,segm_pred1 = process_images(crop_img1, sess, DEEPLAB_INPUT_SIZE, 0.5)\n",
        "\n",
        "### Visualize\n",
        "segm_3ch = crop_img.copy()\n",
        "segm_3ch[:,:,0] = segm_prob\n",
        "segm_3ch[:,:,1] = segm_prob\n",
        "segm_3ch[:,:,2] = segm_prob\n",
        "for pt in lidar_2d:\n",
        "  pt = pt.astype(int)\n",
        "  if segm_pred[pt[1],pt[0]] == 0:\n",
        "    continue\n",
        "  c = (255,0,0)\n",
        "  cv2.circle(segm_3ch,tuple(pt[:2]),1,c,-1)\n",
        "\n",
        "segm_3ch1 = crop_img1.copy()\n",
        "segm_3ch1[:,:,0] = segm_prob1\n",
        "segm_3ch1[:,:,1] = segm_prob1\n",
        "segm_3ch1[:,:,2] = segm_prob1\n",
        "for pt in lidar_2d1:\n",
        "  pt = pt.astype(int)\n",
        "  if segm_pred1[pt[1],pt[0]] == 0:\n",
        "    continue\n",
        "  c = (255,0,0)\n",
        "  cv2.circle(segm_3ch1,tuple(pt[:2]),1,c,-1)\n",
        "\n",
        "fig,axs = plt.subplots(1,2,figsize=(10,7))\n",
        "axs[0].imshow(segm_3ch)\n",
        "axs[1].imshow(segm_3ch1)\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "k5HB34TQc8bC"
      },
      "source": [
        "## LiDAR Road Filter"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "A2BBdmSkA5-4"
      },
      "source": [
        "The road points set that are obtained from the previous step may contain false detection and obviously only contains point that are inside the camera field of view. We also need to detect road points that are outside this set!\n",
        "\n",
        "To do this, fit a plane model (which will represent the road model) $Ax+By+Cz=1$ to the current road points. Any points that are outside the camera field of view and located near the road plane model can be also regarded as road points. RANSAC algorithm is applied when fitting the model to reduce the influence of outliers. Visit this [Wikipedia page](https://en.wikipedia.org/wiki/Random_sample_consensus) for more information about RANSAC.\n",
        "\n",
        "Parameter:\n",
        "- ROAD_HEIGHT_THRESHOLD - The maximum distance (in height axis) between a point to the road model to be considered as road point."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "LKocWSZ9dG-D"
      },
      "outputs": [],
      "source": [
        "### Get the plane model from the road points\n",
        "def get_road_model_ransac(img_pred,lidar_in_cam,lidar_2d):\n",
        "  lidar_in_road_lbl = [True if img_pred[pt[1],pt[0]] == 255 else False for pt in lidar_2d]\n",
        "  lidar_in_road = lidar_in_cam[lidar_in_road_lbl,:]\n",
        "  road_model = RANSACRegressor().fit(lidar_in_road[:,[0,2]],lidar_in_road[:,1])\n",
        "  return road_model\n",
        "\n",
        "### Predict all LiDAR points as road or not\n",
        "def filter_road_points(road_model,lidar_in,threshold=0.5):\n",
        "  x = lidar_in[:,[0,2]]\n",
        "  y_true = lidar_in[:,1]\n",
        "  y_pred = road_model.predict(x)\n",
        "  delta_y = np.absolute(y_true-y_pred).flatten()\n",
        "  is_not_road = delta_y > threshold\n",
        "  lidar_out = lidar_in[is_not_road,:].copy()\n",
        "  return lidar_out\n",
        "\n",
        "road_height_threshold = 0.1\n",
        "\n",
        "road_model = get_road_model_ransac(segm_pred,lidar_in_cam,lidar_2d)\n",
        "lidar_nonroad = filter_road_points(road_model,lidar_raw,road_height_threshold)\n",
        "\n",
        "road_model1 = get_road_model_ransac(segm_pred1,lidar_in_cam1,lidar_2d1)\n",
        "lidar_nonroad1 = filter_road_points(road_model1,lidar_raw1,road_height_threshold)\n",
        "\n",
        "### Visualize\n",
        "fig,axs = plt.subplots(1,2,figsize=(20,10))\n",
        "axs[0].scatter(lidar_nonroad[:,0],lidar_nonroad[:,2],c=-lidar_nonroad[:,1],marker='.')\n",
        "axs[0].scatter(0,0,c='r',marker='x')\n",
        "axs[0].axis('scaled')\n",
        "axs[1].scatter(lidar_nonroad1[:,0],lidar_nonroad1[:,2],c=-lidar_nonroad1[:,1],marker='.')\n",
        "axs[1].scatter(0,0,c='r',marker='x')\n",
        "axs[1].axis('scaled')\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "KEJENqREB-xe"
      },
      "source": [
        "Now we can remove the road points from the LiDAR data and proceed to the mapping system."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "MvjFQgbURjkh"
      },
      "source": [
        "# MAPPING (OGM)\n",
        "\n",
        "The Occupancy Grid Map (OGM) in this tutorial is estimated with the procedure described in [4]. OGM is a grid-based (image-like) map where each of its cell/pixel contains probability of that cell occupied by any obstacle. "
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "84BWoo6ft0Pc"
      },
      "source": [
        "## Parameters"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "g5Ou-U1npNFP"
      },
      "source": [
        "There are several parameters that need to be defined:\n",
        "- ALPHA - The radial resolution when converting LiDAR data to grid map in m (explained later).\n",
        "- BHETA - The angular resolution when converting LiDAR data to grid map in radian (explained later) .\n",
        "- RESOLUTION - The resolution of the grid map in m.\n",
        "- MAX_RANGE - Maximum range of LiDAR points that will be converted to grid map.\n",
        "- MAP_WIDTH - Width of the map from side to side in m.\n",
        "- SPHERICAL2CARTESIAN_BIAS - An adjustment needed due to some errors when converting the spherical grid map to the cartesian grid map."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "h_H-xY32t151"
      },
      "outputs": [],
      "source": [
        "ALPHA = 1\n",
        "BHETA = 1*np.pi/180\n",
        "RESOLUTION = 0.1\n",
        "MAX_RANGE = 50\n",
        "MAP_WIDTH = 100\n",
        "SPHERICAL2CARTESIAN_BIAS = 6"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "FwA7TdDCtnL8"
      },
      "source": [
        "## Map Initialization"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "IzqwlnXphqnF"
      },
      "source": [
        "The OGM is initialized with probability of all of its cells = 0.5"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "dNTcnvRbtmeD"
      },
      "outputs": [],
      "source": [
        "### Some derived parameters\n",
        "# OOR_MASK = A mask to filter out measurements that are out of MAX_RANGE\n",
        "MAP_SIZE_X = int(MAP_WIDTH/RESOLUTION)\n",
        "MAP_SIZE_Y = int(MAP_WIDTH/RESOLUTION)\n",
        "xarr = np.arange(-MAP_WIDTH/2,MAP_WIDTH/2,RESOLUTION)\n",
        "yarr = np.arange(-MAP_WIDTH/2,MAP_WIDTH/2,RESOLUTION)\n",
        "MAP_XX, MAP_YY = np.meshgrid(xarr, -yarr)\n",
        "rgrid = np.sqrt(np.add(np.square(MAP_XX),np.square(MAP_YY)))\n",
        "OOR_MASK = rgrid >= MAX_RANGE\n",
        "\n",
        "### Initialize OGM\n",
        "ogm_time_0 = np.ones((MAP_SIZE_Y,MAP_SIZE_X)) * 0.5\n",
        "\n",
        "### Only use the x-z axis of the point (ignore the height axis)\n",
        "lidar_ogm = lidar_nonroad[:,[2,0]]\n",
        "lidar_ogm1 = lidar_nonroad1[:,[2,0]]\n",
        "\n",
        "### Visualize\n",
        "# Yes, it's still empty\n",
        "plt.figure(figsize=(6, 6))\n",
        "plt.imshow(ogm_time_0,cmap='gray',vmax=1)\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "RgO7kAzqz8Av"
      },
      "source": [
        "## Generate Measurement Grid from Filtered LiDAR"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "3Bn9_Vq7wq17"
      },
      "source": [
        "To update the OGM with the most recent measurement, the LiDAR points need to be converted to the similar grid format. This grid is called the scan grid (SG).\n",
        "\n",
        "Take a look at the figure of a SG below. In the figure, the purple point is a sample of the LiDAR measurements. There are three conditions to fill the SG.\n",
        "1. Black cells: The cells around the point (cells that are radially located at +-ALPHA and angularly located at +-BHETA from the point) are given probability = 0.7 which means that they are likely to be occupied.\n",
        "2. White cells: The cells that are located between the origin/sensor and the measured point are given probability = 0.3 which means that they are likely to be free.\n",
        "3. Gray cells: The other cells, including the ones behind the points, are given probability = 0.5 which means that we can't infer whether they are free or occupied.\n",
        "\n",
        "<br>\n",
        "<center><img src=\"https://github.com/MukhlasAdib/KITTI_Mapping/blob/main/figures/sg_gen.png?raw=true\" width=300px></center>\n",
        "</br>\n",
        "\n",
        "The efficient implementation of the SG creation is not straightforward. In this tutorial, we create the SG in spherical coordinate first. After the spherical SG is filled, then it is converted to cartesian coordinate. I recommend you to read the procedure of scan grid generation in [5]. "
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "tqcwHASlRmI9"
      },
      "outputs": [],
      "source": [
        "def generate_measurement_ogm(lidar_in,ogm_shape):\n",
        "  ### Calculate the position of LiDAR points in spherical coordinate\n",
        "  rphi_meas = np.zeros((lidar_in.shape[0],2))\n",
        "  rphi_meas[:,1] = np.sqrt(np.add(np.square(lidar_in[:,0]),np.square(lidar_in[:,1])))/ALPHA\n",
        "  rphi_meas[:,0] = (np.arctan2(lidar_in[:,1],lidar_in[:,0])+np.pi)/BHETA\n",
        "  rphi_meas = np.unique(rphi_meas.astype(int),axis=0)\n",
        "  rphi_meas = rphi_meas[rphi_meas[:,1]<int(MAX_RANGE/ALPHA),:]\n",
        "  rphi_meas = rphi_meas[rphi_meas[:,0]<int(2*np.pi/BHETA),:]\n",
        "\n",
        "  ### Initiate and fill the spherical scan grid\n",
        "  sg_ang_bin = int(2*np.pi/BHETA)\n",
        "  sg_rng_bin = int(MAX_RANGE/ALPHA)\n",
        "  # Initiation (Condition 3)\n",
        "  scan_grid = np.ones((sg_ang_bin,sg_rng_bin))*0.5\n",
        "  # Condition 1\n",
        "  scan_grid[tuple(rphi_meas.T)] = 0.7\n",
        "  # Condition 2\n",
        "  for ang in range(sg_ang_bin):\n",
        "    ang_arr = rphi_meas[rphi_meas[:,0]==ang,1]\n",
        "    if len(ang_arr) == 0:\n",
        "      scan_grid[ang,:] = 0.3\n",
        "    else:\n",
        "      min_r = np.min(ang_arr)\n",
        "      scan_grid[ang,:min_r] = 0.3\n",
        "  \n",
        "  ### Convert the spherical scan grid to the cartesian one\n",
        "  ogm_sz = (ogm_shape[1],ogm_shape[0])\n",
        "  ogm_cen = (int(ogm_shape[1]/2),int(ogm_shape[0]/2))\n",
        "  radius = (MAX_RANGE/RESOLUTION) + SPHERICAL2CARTESIAN_BIAS\n",
        "  ogm_step = cv2.warpPolar(scan_grid,ogm_sz,ogm_cen,radius,cv2.WARP_INVERSE_MAP)\n",
        "  ogm_step[OOR_MASK] = 0.5\n",
        "  ogm_step = cv2.rotate(ogm_step, cv2.ROTATE_90_CLOCKWISE)\n",
        "  return ogm_step\n",
        "\n",
        "ogm_step = generate_measurement_ogm(lidar_ogm,ogm_time_0.shape)\n",
        "ogm_step1 = generate_measurement_ogm(lidar_ogm1,ogm_time_0.shape)\n",
        "\n",
        "### Visualize\n",
        "fig,axs = plt.subplots(1,2,figsize=(12,6))\n",
        "axs[0].imshow(((1-ogm_step)*255).astype(np.uint8),cmap='gray')\n",
        "axs[1].imshow(((1-ogm_step1)*255).astype(np.uint8),cmap='gray')\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "RZzVVZF47bRb"
      },
      "source": [
        "## Try First Update"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "45nrLWd72kq6"
      },
      "source": [
        "The OGM is updated by the recent scan grid using Bayesian update. According the derivation in [4], if we initiate the OGM as empty grid, the $i$-th cell of OGM can be updated with this simple formula\n",
        "\n",
        "\\begin{align}\n",
        "L_{i,t} = L_{i,t-1} + L^{SG}_{i,t-1}\n",
        "\\end{align}\n",
        "\n",
        "$L_{i,t}$ is the [logit](https://en.wikipedia.org/wiki/Logit) of the $i$-th cell of the updated OGM.\n",
        "\n",
        "$L_{i,t-1}$ is the logit of the $i$-th cell of the previous OGM.\n",
        "\n",
        "$L^{SG}_{i,t-1}$ is the logit of the $i$-th cell of the scan grid that are generated from the latest LiDAR points.  \n",
        "\n",
        "Then, the usable OGM can be found by calculating the inverse-logit of $L_{i,t}$"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "VNGMlHzav7-y"
      },
      "outputs": [],
      "source": [
        "### Calculate the logit function\n",
        "def logit(m):\n",
        "  return np.log(np.divide(m, np.subtract(1, m)))\n",
        "\n",
        "### Calculate the inverse logit function\n",
        "def inverse_logit(m):\n",
        "  return np.divide(np.exp(m),np.add(1,np.exp(m)))\n",
        "\n",
        "### Update the prior OGM with the scan grid (new_ogm)\n",
        "def update_ogm(prior_ogm,new_ogm):\n",
        "  logit_map = logit(new_ogm) + logit(prior_ogm)\n",
        "  out_ogm = inverse_logit(logit_map)\n",
        "  out_ogm[out_ogm>=0.98] = 0.98\n",
        "  out_ogm[out_ogm<=0.02] = 0.02\n",
        "  return out_ogm\n",
        "\n",
        "ogm_time_1 = update_ogm(ogm_step,ogm_time_0)\n",
        "\n",
        "### Visualize\n",
        "fig,axs = plt.subplots(1,1,figsize=(6,6))\n",
        "axs.imshow(((1-ogm_time_1)*255).astype(np.uint8),cmap='gray')\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "wKHWHNSzIhTR"
      },
      "source": [
        "## Load Vehicle's Poses"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "TzugSBVk5OGm"
      },
      "source": [
        "Before the OGM is updated again at the next time step, it need to be shifted according to the movement of the vehicle. Therefore, we need to find the next pose (x-y-yaw) of the vehicle. We do this by using the velocity of the vehicle, and the elapsed time between the last update and the next time step.\n",
        "\n",
        "*Note that in reality, we cannot estimate the vehicle's pose accurately only by using the odometry information (also called as dead-reckoning). You will need a more advanced technique to get an accurate pose estimation*"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "LaiHzx5ABM2L"
      },
      "outputs": [],
      "source": [
        "### Load the vehicle's pose based on velocity data\n",
        "def load_vehicle_pose_vel(data,idx,old_pose,old_idx):\n",
        "  delta_t = (data.timestamps[idx]-data.timestamps[old_idx]).total_seconds()\n",
        "  packet = data.oxts[idx].packet\n",
        "  vf = packet.vf\n",
        "  vr = -packet.vl\n",
        "  pose_f = old_pose[0] + (vf*delta_t)\n",
        "  pose_r = old_pose[1] + (vr*delta_t)\n",
        "  pose_y = packet.yaw - data.oxts[0].packet.yaw\n",
        "  return (pose_f,pose_r,pose_y)\n",
        "\n",
        "\n",
        "pose = load_vehicle_pose_vel(data,idx,(0,0,0),idx)\n",
        "pose1 = load_vehicle_pose_vel(data,idx1,pose,idx)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "XrJZJfSx8UXT"
      },
      "source": [
        "## Shift the OGM"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "xv1IDNcB51ZU"
      },
      "source": [
        "Shift the OGM according to the next pose"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "h_2GHyEr1YV4"
      },
      "outputs": [],
      "source": [
        "# Shift the map according the vehicle's poses\n",
        "def shift_pose_ogm(ogm, init, fin):\n",
        "  ogm_o = ogm.copy()\n",
        "  theta = init[2] /180 * np.pi\n",
        "  rot_m = np.array([[np.cos(theta),np.sin(theta)],[-np.sin(theta),np.cos(theta)]])\n",
        "  trs_m = np.array([[init[0]],[init[1]]])\n",
        "  point = np.array(fin[:2]).reshape((-1,1))\n",
        "  point_1 = (point - trs_m)\n",
        "  point_2 = np.dot(rot_m,-point_1)\n",
        "  delta_theta = (fin[2] - init[2])\n",
        "  delta = np.array([point_2[1,0]/RESOLUTION,point_2[0,0]/RESOLUTION,0])\n",
        "\n",
        "  M = np.array([[1,0,delta[0]],[0,1,-delta[1]]])\n",
        "  dst = cv2.warpAffine(ogm_o,M,(ogm_o.shape[1],ogm_o.shape[0]),borderValue=0.5)\n",
        "  M = cv2.getRotationMatrix2D((ogm_o.shape[1]/2+0.5,ogm_o.shape[0]/2+0.5),delta_theta,1)\n",
        "  dst = cv2.warpAffine(dst,M,(ogm_o.shape[1],ogm_o.shape[0]),borderValue=0.5)\n",
        "  return dst\n",
        "\n",
        "shift_ogm_time_1 = shift_pose_ogm(ogm_time_1,pose,pose1)\n",
        "\n",
        "### Visualize\n",
        "fig,axs = plt.subplots(1,2,figsize=(12,6))\n",
        "axs[0].imshow(((1-ogm_time_1)*255).astype(np.uint8),cmap='gray')\n",
        "axs[0].set_title('Before')\n",
        "axs[1].imshow(((1-shift_ogm_time_1)*255).astype(np.uint8),cmap='gray')\n",
        "axs[1].set_title('After')\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "1NsVJKtwH1fg"
      },
      "source": [
        "## Try Second Update"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "PvVCpXtR6oLZ"
      },
      "source": [
        "Let's try to update the OGM once again!"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "EG1zM2g19Ffo"
      },
      "outputs": [],
      "source": [
        "ogm_time_2 = update_ogm(ogm_step1,shift_ogm_time_1)\n",
        "\n",
        "### Visualize\n",
        "fig,axs = plt.subplots(1,1,figsize=(6,6))\n",
        "axs.imshow(((1-ogm_time_2)*255).astype(np.uint8),cmap='gray')\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "I2ThKoH26A_D"
      },
      "source": [
        "# MAPPING (DGM)\n",
        "\n",
        "In OGM, it is hard to tell which one among the detected obstacles are dynamic objects. Many imporvements have been done to make the OGM can store the information of dynamics object, which leads to a new type of grid map: Dynamic Grid Map (DGM). One simple approach that can be used to generate DGM is by using Dempster-Shafer Theory (DST), also known as evidential theory, to estimate the map instead of using probabilistic Bayesian theory. We implement one based on [5].\n",
        "\n",
        "In DST-based grid map, each cell's state is stored in form of a mass function $m$. This mass function consists of $m(\\{F\\}),m(\\{O\\}),m(\\{F,O\\}),m(\\emptyset)$ that represent evidence that the cell is free, occupied, unknown, or conflicting respectively. The update algorithm for DST-based grid map is performed by using the Dempster's combination rule. In short, the conflicting evidence, which comes up when there are inconsistent measurements at the cell as time passed, may indicate that the cell is occupied by a dynamic object. \n",
        "\n",
        "For more details, please take a look at [5]."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "jAJb3jyM6RSt"
      },
      "source": [
        "## Parameters"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "BicZobjPPP2u"
      },
      "source": [
        "The parameters mostly similar with OGM, but with addition:\n",
        "- FREE_CONF = Our confidence level [0,1] in the results that are categorized as free.\n",
        "- OCC_CONF = Our confidence level [0,1] in the results that are categorized as occupied.\n",
        "- DYNAMIC_THRESHOLD = The minimal value of $m(\\emptyset)$ to categorize a cell as occupied by dynamic object.\n",
        "\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "0XYbJZnS6RSt"
      },
      "outputs": [],
      "source": [
        "ALPHA = 1\n",
        "BHETA = 1*np.pi/180\n",
        "RESOLUTION = 0.1\n",
        "MAX_RANGE = 50\n",
        "MAP_WIDTH = 100\n",
        "SPHERICAL2CARTESIAN_BIAS = 6\n",
        "FREE_CONF = 0.7\n",
        "OCC_CONF = 0.7\n",
        "DYNAMIC_THRESHOLD = 0.1"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "XrmbUStU6RSt"
      },
      "source": [
        "## Map Initialization"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "n_lTHU_CPNHe"
      },
      "source": [
        "The OGM is initialized with mass function of all of its cells, $m(\\{F\\})=0,m(\\{O\\})=0,m(\\{F,O\\})=1,m(\\emptyset)=0$.\n",
        "\n",
        "This means that we still don't have any information about the occupancy of the cells."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "z30qPjM66RSt"
      },
      "outputs": [],
      "source": [
        "MAP_SIZE_X = int(MAP_WIDTH/RESOLUTION)\n",
        "MAP_SIZE_Y = int(MAP_WIDTH/RESOLUTION)\n",
        "xarr = np.arange(-MAP_WIDTH/2,MAP_WIDTH/2,RESOLUTION)\n",
        "yarr = np.arange(-MAP_WIDTH/2,MAP_WIDTH/2,RESOLUTION)\n",
        "MAP_XX, MAP_YY = np.meshgrid(xarr, -yarr)\n",
        "rgrid = np.sqrt(np.add(np.square(MAP_XX),np.square(MAP_YY)))\n",
        "OOR_MASK = rgrid >= MAX_RANGE\n",
        "\n",
        "'''\n",
        "DGM Channel:\n",
        "Channel 0 = {F,O}\n",
        "Channel 1 = O\n",
        "Channel 2 = F\n",
        "'''\n",
        "dgm_time_0 = np.zeros((MAP_SIZE_Y,MAP_SIZE_X,3))\n",
        "dgm_time_0[:,:,0] = 1 \n",
        "\n",
        "lidar_dgm = lidar_nonroad[:,[2,0]]\n",
        "lidar_dgm1 = lidar_nonroad1[:,[2,0]]"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "PZhB3ouU6RSu"
      },
      "source": [
        "## Generate Measurement Grid from Filtered LiDAR"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "06W3ZKzeP7Fu"
      },
      "source": [
        "The overall process of scan grid generation is the same as in OGM. The only difference is how we fill the cells according to condition 1, 2, and 3 (refer to the image in OGM measurement grid generation code). Cell under condition:\n",
        "\n",
        "1. (Black cells/likely to be occupied) - are given mass function \n",
        "\n",
        "\\begin{align}m(\\{F\\})=0, m(\\{O\\})=OCC\\_CONF, m(\\{F,O\\})=1-OCC\\_CONF\\end{align}\n",
        "\n",
        "2. (White cells/likely to be free) - are given mass function \n",
        "\n",
        "\\begin{align}m(\\{F\\})=FREE\\_CONF, m(\\{O\\})=0, m(\\{F,O\\})=1-FREE\\_CONF\\end{align}\n",
        "\n",
        "3. (Gray cells/unknown) - are given mass function \n",
        "\n",
        "\\begin{align}m(\\{F\\})=0, m(\\{O\\})=0, m(\\{F,O\\})=1\\end{align}\n",
        "\n",
        "The value of $m(\\emptyset)$ is 0 and should always be 0, according to DST. We only use $m(\\emptyset)$ to detect dynamic cells at an instant time.\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "HT0gy9BM6RSu"
      },
      "outputs": [],
      "source": [
        "def generate_measurement_dgm(lidar_in,dgm_shape):\n",
        "  ### Calculate the position of LiDAR points in spherical coordinate\n",
        "  rphi_meas = np.zeros((lidar_in.shape[0],2))\n",
        "  rphi_meas[:,1] = np.sqrt(np.add(np.square(lidar_in[:,0]),np.square(lidar_in[:,1])))/ALPHA\n",
        "  rphi_meas[:,0] = (np.arctan2(lidar_in[:,1],lidar_in[:,0])+np.pi)/BHETA\n",
        "  rphi_meas = np.unique(rphi_meas.astype(int),axis=0)\n",
        "  rphi_meas = rphi_meas[rphi_meas[:,1]<int(MAX_RANGE/ALPHA),:]\n",
        "  rphi_meas = rphi_meas[rphi_meas[:,0]<int(2*np.pi/BHETA),:]\n",
        "\n",
        "  ### Initiate and fill the spherical scan grid\n",
        "  sg_ang_bin = int(2*np.pi/BHETA)\n",
        "  sg_rng_bin = int(MAX_RANGE/ALPHA)\n",
        "  scan_grid = np.zeros((sg_ang_bin,sg_rng_bin,3))\n",
        "  scan_grid[:,:,0] = 1 \n",
        "  scan_grid[tuple(rphi_meas.T)] = (1-OCC_CONF,OCC_CONF,0)\n",
        "  for ang in range(sg_ang_bin):\n",
        "    ang_arr = rphi_meas[rphi_meas[:,0]==ang,1]\n",
        "    if len(ang_arr) == 0:\n",
        "      scan_grid[ang,:] = (1-FREE_CONF,0,FREE_CONF)\n",
        "    else:\n",
        "      min_r = np.min(ang_arr)\n",
        "      scan_grid[ang,:min_r] = (1-FREE_CONF,0,FREE_CONF)\n",
        "  \n",
        "  ### Convert the spherical scan grid to the cartesian one\n",
        "  dgm_sz = (dgm_shape[1],dgm_shape[0])\n",
        "  dgm_cen = (int(dgm_shape[1]/2),int(dgm_shape[0]/2))\n",
        "  radius = (MAX_RANGE/RESOLUTION) + SPHERICAL2CARTESIAN_BIAS\n",
        "  dgm_step = cv2.warpPolar(scan_grid,dgm_sz,dgm_cen,radius,cv2.WARP_INVERSE_MAP)\n",
        "  dgm_step[OOR_MASK] = (1,0,0)\n",
        "  dgm_step = cv2.rotate(dgm_step, cv2.ROTATE_90_CLOCKWISE)\n",
        "  return dgm_step\n",
        "\n",
        "dgm_step = generate_measurement_dgm(lidar_dgm,dgm_time_0.shape)\n",
        "dgm_step1 = generate_measurement_dgm(lidar_dgm1,dgm_time_0.shape)\n",
        "\n",
        "### Visualize\n",
        "fig,axs = plt.subplots(1,2,figsize=(12,6))\n",
        "axs[0].imshow((dgm_step*255).astype(np.uint8))\n",
        "axs[1].imshow((dgm_step1*255).astype(np.uint8))\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "R9J6VTpplV_u"
      },
      "source": [
        "## Try First Update"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "LA1u-f7SS6-G"
      },
      "source": [
        "The update of DGM is done with Dempster's rule of combination. By theory, to combine two mass functions $m_1$ and $m_2$ into a new mass function $m_{1,2}$, the following formula is used\n",
        "\n",
        "\\begin{align}\n",
        "m_{1,2}(A)=\\frac{1}{1-K}\\sum_{B\\cap C=A\\neq\\emptyset} m_1(B).m_2(C)\n",
        "\\end{align}\n",
        "\n",
        "\\begin{align}\n",
        "m(\\emptyset) = 0\n",
        "\\end{align}\n",
        "\n",
        "\\begin{align}\n",
        "K=\\sum_{B\\cap C=\\emptyset} m_1(B).m_2(C)\n",
        "\\end{align}\n",
        "\n",
        "Note that te sum of all evindeces in a mass function must be equal to 1. If we apply it to the DGM case, the combination rule will be\n",
        "\n",
        "\\begin{align}\n",
        "m_{t+1}(\\{F\\})=\\frac{1}{1-K}(m_t(\\{F\\}).m^{SG}_t(\\{F,O\\}) + m_t(\\{F,O\\}).m^{SG}_t(\\{F\\}))\n",
        "\\end{align}\n",
        "\n",
        "\\begin{align}\n",
        "m_{t+1}(\\{O\\})=\\frac{1}{1-K}(m_t(\\{O\\}).m^{SG}_t(\\{F,O\\}) + m_t(\\{F,O\\}).m^{SG}_t(\\{O\\}))\n",
        "\\end{align}\n",
        "\n",
        "\\begin{align}\n",
        "m_{t+1}(\\{F,O\\})=\\frac{1}{1-K}(m_t(\\{F,O\\}).m^{SG}_t(\\{F,O\\}))\n",
        "\\end{align}\n",
        "\n",
        "\\begin{align}\n",
        "m_{t+1}(\\emptyset) = 0\n",
        "\\end{align}\n",
        "\n",
        "\\begin{align}\n",
        "K=m_t(\\{O\\}).m^{SG}_t(\\{F\\}) + m_t(\\{F\\}).m^{SG}_t(\\{O\\}))\n",
        "\\end{align}\n",
        "\n",
        "with $m_t$ is the mass function of previous DGM, $m^{SG}_t$ is the scan grid ofthe current LiDAR measurement and $m_{t+1}$ is the updated DGM mass function.The conflicting evidence is actually calculated as $K$. But because the DST only allow $m(\\emptyset)$ to be 0, this factor is used as normalization factor for others. \n",
        "\n",
        "To categorize the cells, we only need to find the maximum mass in its mass function. For example, if the maximum mass is $F$, it will be categorized as free cell. But, cells that have $K>DYNAMIC\\_THRESHOLD$ will be classified as dynamic cells.\n"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "Mo5JbqzICarz"
      },
      "outputs": [],
      "source": [
        "### Update the DGM with DST rule of combination\n",
        "def update_dgm(prior_dgm,new_dgm):\n",
        "  ### Calculate conflicting mass\n",
        "  conflict_mass = np.multiply(prior_dgm[:,:,2],new_dgm[:,:,1])\n",
        "  conflict_mass = np.add(conflict_mass,np.multiply(prior_dgm[:,:,1],new_dgm[:,:,2]))\n",
        "\n",
        "  ### Calculate free mass\n",
        "  free_mass = np.multiply(prior_dgm[:,:,0],new_dgm[:,:,2])\n",
        "  free_mass = np.add(free_mass,np.multiply(prior_dgm[:,:,2],new_dgm[:,:,0]))\n",
        "  free_mass = np.add(free_mass,np.multiply(prior_dgm[:,:,2],new_dgm[:,:,2]))\n",
        "  free_mass = np.divide(free_mass,1-conflict_mass)\n",
        "\n",
        "  ### Calculate occupied mass\n",
        "  occ_mass = np.multiply(prior_dgm[:,:,0],new_dgm[:,:,1])\n",
        "  occ_mass = np.add(occ_mass,np.multiply(prior_dgm[:,:,1],new_dgm[:,:,0]))\n",
        "  occ_mass = np.add(occ_mass,np.multiply(prior_dgm[:,:,1],new_dgm[:,:,1]))\n",
        "  occ_mass = np.divide(occ_mass,1-conflict_mass)\n",
        "\n",
        "  ### Calculate unknown mass\n",
        "  unknown_mass = np.multiply(prior_dgm[:,:,0],new_dgm[:,:,0])\n",
        "  unknown_mass = np.divide(unknown_mass,1-conflict_mass)\n",
        "\n",
        "  updated_dgm = np.stack((unknown_mass,occ_mass,free_mass),axis=2)\n",
        "  return updated_dgm,conflict_mass\n",
        "\n",
        "### Convert the DGM to a displayable figure\n",
        "def predict_dgm(dgm,dynamic_mass):\n",
        "  max_mass = np.argmax(dgm,axis=2)\n",
        "  pred_map = np.zeros(dgm.shape)\n",
        "  # The unknown cells: gray\n",
        "  pred_map[max_mass==0] = (123,123,123)\n",
        "  # The occupied cells: black\n",
        "  pred_map[max_mass==1] = (0,0,0)\n",
        "  # The free cells: white\n",
        "  pred_map[max_mass==2] = (255,255,255)\n",
        "  # The dynamic cells: blue\n",
        "  pred_map[dynamic_mass>=DYNAMIC_THRESHOLD] = (0,0,255)\n",
        "  return pred_map.astype(np.uint8)\n",
        "\n",
        "dgm_time_1,dynamic_mass_1 = update_dgm(dgm_time_0,dgm_step)\n",
        "dgm_pred_1 = predict_dgm(dgm_time_1,dynamic_mass_1)\n",
        "\n",
        "fig,axs = plt.subplots(1,1,figsize=(6,6))\n",
        "axs.imshow(dgm_pred_1)\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "VItwuWo2ljyK"
      },
      "source": [
        "## Load Vehicle's Pose"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "KH2tPpkkbc9I"
      },
      "source": [
        "Get the vehicle's pose just like in OGM"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "k3A3tq0OkjlB"
      },
      "outputs": [],
      "source": [
        "### Load the vehicle's pose based on velocity data\n",
        "def load_vehicle_pose_vel(data,idx,old_pose,old_idx):\n",
        "  delta_t = (data.timestamps[idx]-data.timestamps[old_idx]).total_seconds()\n",
        "  packet = data.oxts[idx].packet\n",
        "  vf = packet.vf\n",
        "  vr = -packet.vl\n",
        "  pose_f = old_pose[0] + (vf*delta_t)\n",
        "  pose_r = old_pose[1] + (vr*delta_t)\n",
        "  pose_y = packet.yaw - data.oxts[0].packet.yaw\n",
        "  return (pose_f,pose_r,pose_y)\n",
        "\n",
        "pose = load_vehicle_pose_vel(data,idx,(0,0,0),idx)\n",
        "pose1 = load_vehicle_pose_vel(data,idx1,pose,idx)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "gYeJ8xxUlo_J"
      },
      "source": [
        "## Shift the DGM"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "Ssk0GPZmbg93"
      },
      "source": [
        "Shift the OGM with the same way as with OGM"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "OwgvCoqqliyJ"
      },
      "outputs": [],
      "source": [
        "# Shift the map according the vehicle's poses\n",
        "def shift_pose_dgm(dgm, init, fin):\n",
        "  dgm_o = dgm.copy()\n",
        "  theta = init[2] /180 * np.pi\n",
        "  rot_m = np.array([[np.cos(theta),np.sin(theta)],[-np.sin(theta),np.cos(theta)]])\n",
        "  trs_m = np.array([[init[0]],[init[1]]])\n",
        "  point = np.array(fin[:2]).reshape((-1,1))\n",
        "  point_1 = (point - trs_m)\n",
        "  point_2 = np.dot(rot_m,-point_1)\n",
        "  delta_theta = (fin[2] - init[2])\n",
        "  delta = np.array([point_2[1,0]/RESOLUTION,point_2[0,0]/RESOLUTION,0])\n",
        "\n",
        "  M = np.array([[1,0,delta[0]],[0,1,-delta[1]]])\n",
        "  dst = cv2.warpAffine(dgm_o,M,(dgm_o.shape[1],dgm_o.shape[0]),borderValue=0.5)\n",
        "  M = cv2.getRotationMatrix2D((dgm_o.shape[1]/2+0.5,dgm_o.shape[0]/2+0.5),delta_theta,1)\n",
        "  dst = cv2.warpAffine(dst,M,(dgm_o.shape[1],dgm_o.shape[0]),borderValue=0.5)\n",
        "  return dst\n",
        "\n",
        "shift_dgm_time_1 = shift_pose_dgm(dgm_time_1,pose,pose1)\n",
        "\n",
        "### Visualize\n",
        "fig,axs = plt.subplots(1,2,figsize=(12,6))\n",
        "axs[0].imshow(((1-ogm_time_1)*255).astype(np.uint8),cmap='gray')\n",
        "axs[0].set_title('Before')\n",
        "axs[1].imshow(((1-shift_ogm_time_1)*255).astype(np.uint8),cmap='gray')\n",
        "axs[1].set_title('After')\n",
        "plt.show()"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "2LWBhx2Yl_-k"
      },
      "source": [
        "## The Second Update"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "uBzvh-8VbmZF"
      },
      "source": [
        "Let's update the map again!\n",
        "\n",
        "The dynamic cells are colored blue. "
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "TuJUZl4Zl8fI"
      },
      "outputs": [],
      "source": [
        "dgm_time_2,dynamic_mass_2 = update_dgm(shift_dgm_time_1,dgm_step1)\n",
        "dgm_pred_2 = predict_dgm(dgm_time_2,dynamic_mass_2)\n",
        "\n",
        "fig,axs = plt.subplots(1,1,figsize=(6,6))\n",
        "axs.imshow(dgm_pred_2)\n",
        "plt.show()"
      ]
    }
  ],
  "metadata": {
    "colab": {
      "authorship_tag": "ABX9TyPMDVh1fO0UnfIxvnO1jqzc",
      "collapsed_sections": [
        "iderI-dPJ7QJ",
        "cDiDJTa7J-fB",
        "uVQYbI6wAbdv",
        "y2sVVXDiAfw5",
        "3QTNtHYfCs2e",
        "1aTnCvzTGGO-",
        "zhy-xR-xKPu6",
        "k5HB34TQc8bC",
        "MvjFQgbURjkh",
        "84BWoo6ft0Pc",
        "FwA7TdDCtnL8",
        "RgO7kAzqz8Av",
        "RZzVVZF47bRb",
        "wKHWHNSzIhTR",
        "XrJZJfSx8UXT",
        "1NsVJKtwH1fg",
        "I2ThKoH26A_D",
        "jAJb3jyM6RSt",
        "XrmbUStU6RSt",
        "PZhB3ouU6RSu",
        "R9J6VTpplV_u",
        "VItwuWo2ljyK",
        "gYeJ8xxUlo_J",
        "2LWBhx2Yl_-k"
      ],
      "include_colab_link": true,
      "name": "KITTI Mapping Tutorial Step-by-step.ipynb",
      "provenance": []
    },
    "kernelspec": {
      "display_name": "Python 3.8.10 ('.env': venv)",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "name": "python",
      "version": "3.8.10"
    },
    "vscode": {
      "interpreter": {
        "hash": "4147159148f2331f27fc35f83760ac3303b0eb37178f521715f234bfc6af8027"
      }
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}