KITTI_Mapping_Tutorial_Full_Loop.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_Full_Loop.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "HfNww3fpJClZ"
      },
      "source": [
        "# REFERENCES\n",
        "\n",
        "**Explanations about each steps are provided in the step-by-step tutorial notebook**\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": "GyNeINdL-c44"
      },
      "source": [
        "# PREPARATION"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "sfaHuhN8-Hhc"
      },
      "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": "1KUxLJ1r-SYq"
      },
      "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\n",
        "from time import sleep\n",
        "from IPython.display import clear_output\n",
        "import zipfile"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "pFkzmei4-i8n"
      },
      "source": [
        "# FUNCTIONS"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "2f_ZihTN-x9S"
      },
      "source": [
        "## Functions for Perception System"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "sOEcqWPr-pky"
      },
      "outputs": [],
      "source": [
        "def load_data(data,idx):\n",
        "  img_raw = np.array(data.get_cam2(idx))\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",
        "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",
        "def project_lidar2cam(lidar_in_cam,camera_intrinsic,img_raw_size):\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",
        "  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",
        "  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",
        "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",
        "  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",
        "  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",
        "  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",
        "  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",
        "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",
        "  if INPUT_SIZE == 0:\n",
        "    resized_image = image.copy()\n",
        "  else:\n",
        "    resized_image = cv2.resize(image,(INPUT_SIZE,INPUT_SIZE))\n",
        "\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",
        "  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",
        "  _,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",
        "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",
        "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",
        "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)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "yU5Owzo0_lm_"
      },
      "source": [
        "## Functions for OGM System"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "XjisBAS5_pFc"
      },
      "outputs": [],
      "source": [
        "def generate_measurement_ogm(lidar_in,ogm_shape):\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",
        "  sg_ang_bin = int(2*np.pi/BHETA)\n",
        "  sg_rng_bin = int(MAX_RANGE/ALPHA)\n",
        "  scan_grid = np.ones((sg_ang_bin,sg_rng_bin))*0.5\n",
        "  scan_grid[tuple(rphi_meas.T)] = 0.7\n",
        "  \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",
        "  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",
        "def logit(m):\n",
        "  return np.log(np.divide(m, np.subtract(1, m)))\n",
        "\n",
        "def inverse_logit(m):\n",
        "  return np.divide(np.exp(m),np.add(1,np.exp(m)))\n",
        "\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",
        "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"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "XzqJ3V7TmsUC"
      },
      "source": [
        "## Functions for DGM System"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "FX0tb3HJmwSH"
      },
      "outputs": [],
      "source": [
        "def generate_measurement_dgm(lidar_in,dgm_shape):\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",
        "  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",
        "  \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",
        "  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",
        "def update_dgm(prior_dgm,new_dgm):\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",
        "  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",
        "  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",
        "  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",
        "def predict_dgm(dgm,dynamic_mass):\n",
        "  max_mass = np.argmax(dgm,axis=2)\n",
        "  pred_map = np.zeros(dgm.shape)\n",
        "  pred_map[max_mass==0] = (123,123,123)\n",
        "  pred_map[max_mass==1] = (0,0,0)\n",
        "  pred_map[max_mass==2] = (255,255,255)\n",
        "  pred_map[dynamic_mass>=DYNAMIC_THRESHOLD] = (0,0,255)\n",
        "  return pred_map.astype(np.uint8)\n",
        "\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"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eSp_OU6gADYH"
      },
      "source": [
        "# SYSTEM LOOP (OGM)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "FuhpfH7IKFyG"
      },
      "source": [
        "## Function for Single OGM Update"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "zzky9eI6AGt3"
      },
      "outputs": [],
      "source": [
        "### Processed according to the step-by-step tutorial\n",
        "def single_loop_ogm(data,idx,tf_sess,ogm):\n",
        "  '''\n",
        "  Args:\n",
        "    data = pykitti object that has been loaded\n",
        "    idx = index of the processed frame\n",
        "    tf_sess = TensorFlow session with loaded DeepLabv3+ model\n",
        "    ogm = the latest estimated OGM\n",
        "  Returns:\n",
        "    updated_ogm = the updated OGM\n",
        "    pose = the latest pose of the vehicle\n",
        "    crop_img = the cropped camera image\n",
        "  Note:\n",
        "    Other parameters are defined globally\n",
        "  '''\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",
        "  lidar_2d,lidar_in_cam = project_lidar2cam(lidar_raw,CAMERA_INTRINSIC,img_raw_size)\n",
        "  crop_img,lidar_2d,lidar_in_cam = crop_data(img_raw,lidar_2d,lidar_in_cam,CROP_RH,CROP_RW)\n",
        "  _,segm_pred = process_images(crop_img, tf_sess, DEEPLAB_INPUT_SIZE, 0.5)\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",
        "  lidar_ogm = lidar_nonroad[:,[2,0]]\n",
        "\n",
        "  pose = load_vehicle_pose_vel(data,idx,OLD_POSE,OLD_IDX)\n",
        "  shifted_ogm = shift_pose_ogm(ogm,OLD_POSE,pose)\n",
        "  ogm_step = generate_measurement_ogm(lidar_ogm,ogm.shape)\n",
        "  updated_ogm = update_ogm(shifted_ogm,ogm_step)\n",
        "\n",
        "  return updated_ogm,pose,crop_img"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "CpvlF7bwKKJI"
      },
      "source": [
        "## Data and Parameters"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "o1o-N_GBA3Z3"
      },
      "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",
        "NUMBER_DATA = len(data.oxts)\n",
        "\n",
        "### Global parameters (Perception)\n",
        "LIDAR2CAM_EXTRINSIC = data.calib.T_cam2_velo\n",
        "CAMERA_INTRINSIC = data.calib.K_cam2\n",
        "CROP_RH = 3\n",
        "CROP_RW = 4\n",
        "DEEPLAB_MODEL_PATH = 'KITTI_Mapping/pretrained/deeplab_model.pb'\n",
        "DEEPLAB_INPUT_SIZE = 513\n",
        "ROAD_HEIGHT_THRESHOLD = 0.15\n",
        "\n",
        "### Global parameters (OGM)\n",
        "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",
        "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"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "aOR0hy9vKWia"
      },
      "source": [
        "## Main Loop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "vn-lrPuZHuK9"
      },
      "outputs": [],
      "source": [
        "is_save = False\n",
        "save_dir = '/content/results_ogm/'\n",
        "if not os.path.exists(save_dir): os.makedirs(save_dir)\n",
        "\n",
        "### Load DeepLab v3+ model\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",
        "### Initiate OGM\n",
        "ogm = np.ones((MAP_SIZE_Y,MAP_SIZE_X)) * 0.5\n",
        "\n",
        "### Process all the data in sequence\n",
        "idx = 0\n",
        "OLD_IDX = 0\n",
        "OLD_POSE = (0,0,0)\n",
        "frequency = 1\n",
        "\n",
        "while True:\n",
        "  if idx >= NUMBER_DATA: break\n",
        "  ogm,pose,camera_img = single_loop_ogm(data,idx,sess,ogm)\n",
        "  OLD_IDX = idx\n",
        "  OLD_POSE = pose\n",
        "  idx = idx + frequency\n",
        "\n",
        "  ### Visualize\n",
        "  fig,axs = plt.subplots(1,2,figsize=(16,8))\n",
        "  ogm_img = ((1-ogm)*255).astype(np.uint8)\n",
        "  ogm_img = cv2.resize(ogm_img,(500,500))\n",
        "  ogm_img = cv2.cvtColor(ogm_img,cv2.COLOR_GRAY2RGB)\n",
        "  center = (int(ogm_img.shape[1]/2),int(ogm_img.shape[0]/2)) \n",
        "  cv2.circle(ogm_img,tuple(center[:2]),5,(255,0,0),-1)\n",
        "  axs[0].imshow(ogm_img,cmap='gray',vmin=0,vmax=255)\n",
        "  axs[1].imshow(camera_img)\n",
        "  axs[0].set_axis_off()\n",
        "  axs[1].set_axis_off()\n",
        "  if is_save:\n",
        "    plt.savefig(f'{save_dir}{OLD_IDX:03d}.png')\n",
        "    plt.close(fig)\n",
        "  else:\n",
        "    plt.show()\n",
        "    clear_output(wait=True)\n",
        "\n",
        "if is_save:\n",
        "  zip_file = f'/content/ogm_results.zip'\n",
        "  with zipfile.ZipFile(zip_file, 'w') as z:\n",
        "    list_f = os.listdir(save_dir)\n",
        "    for fl in list_f:\n",
        "      z.write(save_dir+fl,fl)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "JWZPTmygnfYO"
      },
      "source": [
        "# SYSTEM LOOP (DGM)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "6OgT7wZ8nfYP"
      },
      "source": [
        "## Function for Single DGM Update"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "OHhbCcxqnfYQ"
      },
      "outputs": [],
      "source": [
        "### Processed according to the step-by-step tutorial\n",
        "def single_loop_dgm(data,idx,tf_sess,dgm):\n",
        "  '''\n",
        "  Args:\n",
        "    data = pykitti object that has been loaded\n",
        "    idx = index of the processed frame\n",
        "    tf_sess = TensorFlow session with loaded DeepLabv3+ model\n",
        "    dgm = the latest estimated DGM\n",
        "  Returns:\n",
        "    updated_dgm = the updated DGM (:,:,3)\n",
        "    dynamic_mass = the conflicting mass map (:,:,1)\n",
        "    pose = the latest pose of the vehicle\n",
        "    crop_img = the cropped camera image\n",
        "  Note:\n",
        "    Other parameters are defined globally\n",
        "  '''\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",
        "  lidar_2d,lidar_in_cam = project_lidar2cam(lidar_raw,CAMERA_INTRINSIC,img_raw_size)\n",
        "  crop_img,lidar_2d,lidar_in_cam = crop_data(img_raw,lidar_2d,lidar_in_cam,CROP_RH,CROP_RW)\n",
        "  _,segm_pred = process_images(crop_img, tf_sess, DEEPLAB_INPUT_SIZE, 0.5)\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",
        "  lidar_dgm = lidar_nonroad[:,[2,0]]\n",
        "\n",
        "  pose = load_vehicle_pose_vel(data,idx,OLD_POSE,OLD_IDX)\n",
        "  shifted_dgm = shift_pose_dgm(dgm,OLD_POSE,pose)\n",
        "  dgm_step = generate_measurement_dgm(lidar_dgm,dgm.shape)\n",
        "  updated_dgm,dynamic_mass = update_dgm(shifted_dgm,dgm_step)\n",
        "\n",
        "  return updated_dgm,dynamic_mass,pose,crop_img"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "U_OdwwxCnfYQ"
      },
      "source": [
        "## Data and Parameters"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "jTeV4IcVnfYQ"
      },
      "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",
        "NUMBER_DATA = len(data.oxts)\n",
        "\n",
        "### Global parameters (Perception)\n",
        "LIDAR2CAM_EXTRINSIC = data.calib.T_cam2_velo\n",
        "CAMERA_INTRINSIC = data.calib.K_cam2\n",
        "CROP_RH = 3\n",
        "CROP_RW = 4\n",
        "DEEPLAB_MODEL_PATH = 'KITTI_Mapping/pretrained/deeplab_model.pb'\n",
        "DEEPLAB_INPUT_SIZE = 513\n",
        "ROAD_HEIGHT_THRESHOLD = 0.15\n",
        "\n",
        "### Global parameters (DGM)\n",
        "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",
        "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",
        "FREE_CONF = 0.7\n",
        "OCC_CONF = 0.7\n",
        "DYNAMIC_THRESHOLD = 0.1"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "60HHruSInfYQ"
      },
      "source": [
        "## Main Loop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "7cuXG7OJnfYQ"
      },
      "outputs": [],
      "source": [
        "is_save = False\n",
        "save_dir = '/content/results_dgm/'\n",
        "if not os.path.exists(save_dir): os.makedirs(save_dir)\n",
        "\n",
        "### Load DeepLab v3+ model\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",
        "### Initiate OGM\n",
        "dgm = np.zeros((MAP_SIZE_Y,MAP_SIZE_X,3))\n",
        "dgm[:,:,0] = 1 \n",
        "\n",
        "### Process all the data in sequence\n",
        "idx = 0\n",
        "OLD_IDX = 0\n",
        "OLD_POSE = (0,0,0)\n",
        "frequency = 1\n",
        "\n",
        "while True:\n",
        "  if idx >= NUMBER_DATA: break\n",
        "  dgm,dynamic_mass,pose,camera_img = single_loop_dgm(data,idx,sess,dgm)\n",
        "  OLD_IDX = idx\n",
        "  OLD_POSE = pose\n",
        "  idx = idx + frequency\n",
        "\n",
        "  ### Visualize\n",
        "  dgm_pred = predict_dgm(dgm,dynamic_mass)\n",
        "  fig,axs = plt.subplots(1,2,figsize=(16,8))\n",
        "  dgm_pred = cv2.resize(dgm_pred,(500,500))\n",
        "  center = (int(dgm_pred.shape[1]/2),int(dgm_pred.shape[0]/2)) \n",
        "  cv2.circle(dgm_pred,tuple(center[:2]),5,(255,0,0),-1)\n",
        "  axs[0].imshow(dgm_pred,vmin=0,vmax=255)\n",
        "  axs[1].imshow(camera_img)\n",
        "  axs[0].set_axis_off()\n",
        "  axs[1].set_axis_off()\n",
        "  if is_save:\n",
        "    plt.savefig(f'{save_dir}{OLD_IDX:03d}.png')\n",
        "    plt.close(fig)\n",
        "  else:\n",
        "    plt.show()\n",
        "    clear_output(wait=True)\n",
        "\n",
        "if is_save:\n",
        "  zip_file = f'/content/dgm_results.zip'\n",
        "  with zipfile.ZipFile(zip_file, 'w') as z:\n",
        "    list_f = os.listdir(save_dir)\n",
        "    for fl in list_f:\n",
        "      z.write(save_dir+fl,fl)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "aZk77qipnfYQ"
      },
      "outputs": [],
      "source": []
    }
  ],
  "metadata": {
    "colab": {
      "authorship_tag": "ABX9TyPLK/l4A0yxDAao+hfc8ZhW",
      "collapsed_sections": [
        "GyNeINdL-c44",
        "pFkzmei4-i8n",
        "2f_ZihTN-x9S",
        "yU5Owzo0_lm_",
        "XzqJ3V7TmsUC",
        "eSp_OU6gADYH",
        "FuhpfH7IKFyG",
        "CpvlF7bwKKJI",
        "aOR0hy9vKWia",
        "JWZPTmygnfYO",
        "6OgT7wZ8nfYP",
        "U_OdwwxCnfYQ",
        "60HHruSInfYQ"
      ],
      "include_colab_link": true,
      "name": "KITTI Mapping Tutorial Full Loop.ipynb",
      "provenance": []
    },
    "kernelspec": {
      "display_name": "Python 3.8.10 ('ido_gas_station')",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "name": "python",
      "version": "3.8.10"
    },
    "vscode": {
      "interpreter": {
        "hash": "a4f243fdca4ecc4b48861c26f16032bcae36fa461a316778c72089570de783a1"
      }
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}