Det_SLAM.ipynb

{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "view-in-github",
        "colab_type": "text"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/aseslamian/Det-SLAM/blob/main/Det_SLAM.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "efj8pCr5-0io"
      },
      "source": [
        "# Det-SLAM: A semantic visual SLAM for highly dynamic scene using Detectron2\n"
      ]
    },
    {
      "cell_type": "markdown",
      "source": [
        "### Part A) Semantic Segmentation"
      ],
      "metadata": {
        "id": "uCMWvL2DSvFn"
      }
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "e7oYaQiy_sv4"
      },
      "source": [
        "step-1) Import Detectron2 library in colab in order to semantic segmentation of \n",
        "moving Objects. We use pre-train weights  \"COCO-InstanceSegmentation/\n",
        "mask_rcnn_X_101_32x8d_FPN_3xl\" to configure system for selecting the most known moving objects in datasets."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "h-ZZmVUS-z5Q"
      },
      "outputs": [],
      "source": [
        "!python -m pip install pyyaml==5.1\n",
        "!python -m pip install 'git+https://github.com/facebookresearch/detectron2.git'\n",
        "\n",
        "import torch, detectron2\n",
        "!nvcc --version\n",
        "TORCH_VERSION = \".\".join(torch.__version__.split(\".\")[:2])\n",
        "CUDA_VERSION = torch.__version__.split(\"+\")[-1]\n",
        "\n",
        "import detectron2\n",
        "from detectron2.utils.logger import setup_logger\n",
        "setup_logger()\n",
        "# import some common libraries\n",
        "import numpy as np\n",
        "import os, json, cv2, random \n",
        "from google.colab.patches import cv2_imshow\n",
        "# import some common detectron2 utilities\n",
        "from detectron2 import model_zoo\n",
        "from detectron2.engine import DefaultPredictor\n",
        "from detectron2.config import get_cfg\n",
        "from detectron2.utils.visualizer import Visualizer, ColorMode\n",
        "from detectron2.data import MetadataCatalog, DatasetCatalog\n",
        "## Configur system\n",
        "cfg = get_cfg()\n",
        "cfg.merge_from_file(model_zoo.get_config_file(\"COCO-InstanceSegmentation/mask_rcnn_X_101_32x8d_FPN_3x.yaml\"))\n",
        "cfg.MODEL.ROI_HEADS.SCORE_THRESH_TEST = 0.5  # set threshold for this model\n",
        "cfg.MODEL.WEIGHTS = model_zoo.get_checkpoint_url(\"COCO-InstanceSegmentation/mask_rcnn_X_101_32x8d_FPN_3x.yaml\")\n",
        "predictor = DefaultPredictor(cfg)"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "5Fvt47AiBwXd"
      },
      "source": [
        "step-2) Use the configure system for removing objects. In each picture moving object is recognized and covered with a black mask. So selected part can not be considered in furthur processing of ORB-SLAM3"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "8YHttAnjBwfm"
      },
      "outputs": [],
      "source": [
        "import cv2\n",
        "import os\n",
        "from PIL import Image\n",
        "\n",
        "def load_images_from_folder(folder,folder2):\n",
        "    images = []\n",
        "    for filename in os.listdir(folder):\n",
        "        img = cv2.imread(os.path.join(folder,filename))\n",
        "        outputs = predictor(img)\n",
        "        masks = np.asarray(outputs[\"instances\"].pred_masks.to(\"cpu\"))\n",
        "        label = np.array(outputs[\"instances\"].pred_classes.to(\"cpu\"))\n",
        "        for i in range(masks.shape[0]):\n",
        "            if label[i] == 0  or label[i] == 56:\n",
        "                    item_mask = masks[i]\n",
        "                    cropped_mask = Image.fromarray((item_mask * 255).astype('uint8'))\n",
        "                    p = np.array(cropped_mask)\n",
        "                    p.shape = p.shape + (1,)\n",
        "                    p = (255- p)/255\n",
        "                    img = np.multiply(p,img)\n",
        "   \n",
        "        cv2.imwrite(os.path.join(folder2,filename), img)\n",
        "    return outputs"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "YCRwK9p3I36s"
      },
      "source": [
        "step-3) Download TUM RGB-D datasets from the source and execute detectron2 on them."
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "import tarfile \n",
        "print(\"press the number of input datasets: \\n1)walking_static\\n2)siting_static\\n3)walking_xyz\\n4)walking_rpy\\n5)walking_half\\n  (example:4)\")\n",
        "num = int(input())\n",
        "if num <1 or num>5:\n",
        "    print(\"Error: (number should be between 1 to 5)\")\n",
        "elif num == 1:\n",
        "     !wget https://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_walking_static.tgz\n",
        "     file = tarfile.open('rgbd_dataset_freiburg3_walking_static.tgz')\n",
        "     file.extractall('/content/') \n",
        "     file.close()\n",
        "elif num == 2:\n",
        "     !wget https://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_sitting_static.tgz\n",
        "     file = tarfile.open('rgbd_dataset_freiburg3_sitting_static.tgz')\n",
        "     file.extractall('/content/') \n",
        "     file.close()\n",
        "elif num == 3:\n",
        "     !wget https://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_walking_xyz.tgz\n",
        "     file = tarfile.open('rgbd_dataset_freiburg3_walking_xyz.tgz')\n",
        "     file.extractall('/content/') \n",
        "     file.close()\n",
        "elif num == 4:\n",
        "      !wget https://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_walking_rpy.tgz\n",
        "      file = tarfile.open('rgbd_dataset_freiburg3_walking_rpy.tgz')\n",
        "      file.extractall('/content/') \n",
        "      file.close()\n",
        "else:\n",
        "      !wget https://vision.in.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_walking_halfsphere.tgz\n",
        "      file = tarfile.open('rgbd_dataset_freiburg3_walking_halfsphere.tgz')\n",
        "      file.extractall('/content/') \n",
        "      file.close()"
      ],
      "metadata": {
        "id": "jEU1DPXdL-0C"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "Hj6wHH-EMd6L"
      },
      "outputs": [],
      "source": [
        "file = str(file.name.replace('.tgz','/rgb'))\n",
        "load_images_from_folder(file , file)"
      ]
    },
    {
      "cell_type": "markdown",
      "source": [
        "Now RGB-D datasets are processed by the Detectron2."
      ],
      "metadata": {
        "id": "NymPQpQ8gSCb"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "### Part B) Depth Processing"
      ],
      "metadata": {
        "id": "ZCi2YmiwOEgu"
      }
    },
    {
      "cell_type": "markdown",
      "source": [
        "Now let's start **Depth Processing**:"
      ],
      "metadata": {
        "id": "8wuZZOJKOZwA"
      }
    },
    {
      "cell_type": "code",
      "source": [
        "import numpy\n",
        "import array as ay\n",
        "## Convert Depth Image from RGB to Grayscale\n",
        "def rgb2gray(rgb):\n",
        "    return np.dot(rgb[...,:3], [0.2989, 0.5870, 0.1140])\n",
        "###\n",
        "def load_depthIMG_from_folder(folder,folder2):\n",
        "    depthImg=[]\n",
        "    for filename in os.listdir(folder):\n",
        "        depthImg = cv2.imread(os.path.join(folder,filename))\n",
        "        outputs = predictor(depthImg)\n",
        "        depthImg = rgb2gray(depthImg)\n",
        "        masks = np.asarray(outputs[\"instances\"].pred_masks.to(\"cpu\"))\n",
        "        bbox  = np.array(outputs[\"instances\"].pred_boxes.to(\"cpu\"))\n",
        "        label = np.array(outputs[\"instances\"].pred_classes.to(\"cpu\"))\n",
        "\n",
        "        for i in range(masks.shape[0]):\n",
        "                if label[i] == 0 :\n",
        "                      item_mask = masks[i]  \n",
        "                      cropped_mask = Image.fromarray((item_mask * 1).astype('uint16'))\n",
        "                      objMask = np.array(cropped_mask)\n",
        "                      pic = depthImg.copy()\n",
        "                      pic= np.multiply(objMask,pic)\n",
        "                      MinObj = np.amin(np.array(pic)[pic != 0])     # np.amin(np.array(pic)[pic != np.amin(pic)])\n",
        "                      MaxObj = np.amax(pic)\n",
        "\n",
        "                      box = np.array(bbox[i].to(\"cpu\"))  \n",
        "                      start_point = (round(box[0]),round(box[1]))\n",
        "                      end_point = (round(box[2]),round(box[3]))\n",
        "                      [height,width] = depthImg.shape\n",
        "                      ROImask = np.zeros((height,width), np.uint16)\n",
        "                      cv2.rectangle(ROImask, start_point,end_point,(1,1),-1);\n",
        "                      pic = depthImg.copy()\n",
        "                      pic = np.multiply(ROImask,pic)\n",
        "                      MinROI = np.amin(np.array(pic)[pic != 0])     # np.amin(np.array(pic)[pic != np.amin(pic)])   \n",
        "                      MaxROI = np.amax(pic)\n",
        "\n",
        "                      alpha = 0.001   ## Should be determined\n",
        "                      d = np.subtract(MaxROI, MaxObj)\n",
        "                      alpha_d= np.dot(alpha, d)\n",
        "                      Min_obj2 = np.subtract(MinObj,alpha_d)\n",
        "                      Max_obj2 = np.add(MaxObj, alpha_d)\n",
        "                      for m in range(height):\n",
        "                          for n in range(width):\n",
        "                              if pic[m,n]>= Min_obj2  and  pic[m,n]<=Max_obj2:\n",
        "                                    depthImg[m,n]=0\n",
        "\n",
        "                     \n",
        "        depthImg = Image.fromarray(depthImg.astype('uint16'))\n",
        "        depthImg = np.array(depthImg)\n",
        "        cv2.imwrite(os.path.join(folder2,filename), depthImg)"
      ],
      "metadata": {
        "id": "DKCoL6jpBitF"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [
        "file = str(file.name.replace('.tgz','/depth'))\n",
        "load_depthIMG_from_folder(file , file)"
      ],
      "metadata": {
        "id": "KKXgBFlLQaSP"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "source": [
        "## Download The Processed Dataset "
      ],
      "metadata": {
        "id": "zFlMneWCP4PM"
      }
    },
    {
      "cell_type": "code",
      "source": [
        "path = '/content/rgbd_dataset_freiburg3_walking_halfsphere.zip'\n",
        "ZipFile  = os.path.join(path,'.zip')\n",
        "files.download(ZipFile)"
      ],
      "metadata": {
        "id": "20VpUT2nta6m"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [
        "import shutil\n",
        "from google.colab import files\n",
        "shutil.make_archive(\"/content/rgbd_dataset_freiburg3_walking_halfsphere\", 'zip', \"/content/rgbd_dataset_freiburg3_walking_halfsphere\")\n",
        "files.download(ZipFile)"
      ],
      "metadata": {
        "id": "uc4QGJ_eZLHQ"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "markdown",
      "source": [
        "Now You can use these data for ORB-SLAM3 in order to mitigate moving objects"
      ],
      "metadata": {
        "id": "ai11Wq6uRZs4"
      }
    }
  ],
  "metadata": {
    "accelerator": "GPU",
    "colab": {
      "provenance": [],
      "authorship_tag": "ABX9TyM1/MxNRTgG/lnEeSbOAdaW",
      "include_colab_link": true
    },
    "kernelspec": {
      "display_name": "Python 3",
      "name": "python3"
    },
    "language_info": {
      "name": "python"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}