SG_fusion_saver.ipynb

{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "view-in-github",
        "colab_type": "text"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/jiheddachraoui/occupancy_grid_generator/blob/main/SG_fusion_saver.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "iderI-dPJ7QJ"
      },
      "source": [
        "# PREPARATION"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "FBjbCBkk6gI1",
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "outputId": "567891bd-36b2-44a3-cd16-1327371d92b4"
      },
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "Mounted at /content/drive\n"
          ]
        }
      ],
      "source": [
        "import os\n",
        "import sys\n",
        "import csv\n",
        "import imageio\n",
        "import numpy as np\n",
        "import cv2\n",
        "import matplotlib.pyplot as plt\n",
        "import time\n",
        "from collections import Counter\n",
        "import tensorflow as tf\n",
        "from sklearn.linear_model import RANSACRegressor\n",
        "from scipy import stats\n",
        "from google.colab import drive\n",
        "drive.mount('/content/drive',force_remount=True)\n"
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "datadir = '/content/drive/MyDrive/s_data/'\n",
        "scenario_file='scenario_test2_gt/'\n",
        "scenario='ground truth/'+scenario_file\n",
        "output='output/'+scenario_file\n",
        "\n",
        "\n",
        "for item in os.listdir(os.path.join(datadir,scenario)):\n",
        "    # Construct the full path to the item\n",
        "    item_path = os.path.join(os.path.join(datadir,scenario), item)\n",
        "    # Check if the item is a directory\n",
        "    if os.path.isdir(item_path) and item != '.ipynb_checkpoints':\n",
        "        # Print the path of the subdirectory\n",
        "        print(item_path)\n",
        "\n"
      ],
      "metadata": {
        "id": "gu1FbSfH4R6f",
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "outputId": "28087fd1-14a4-4290-c2bf-c67a133437f0"
      },
      "execution_count": null,
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "/content/drive/MyDrive/s_data/ground truth/scenario_test2_gt/hero vehicle.lincoln.mkz_2017\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario_test2_gt/adversary vehicle.tesla.model3\n"
          ]
        }
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "REDUCED_LABEL_COLORS = np.array([\n",
        "    (255, 255, 255), # None\n",
        "    (128, 64, 128),  # Road RoadLines\n",
        "    (80, 80, 80),    # Building,Wall, static ,Fences\n",
        "    (250, 170, 30),    # Other ,GuardRail ,RailTrack ,15Bridge\n",
        "    (110, 190, 160), # Pole ;TrafficLight; Static\n",
        "    (107, 142, 35),  # Vegetation\n",
        "    (0, 0, 142),     # Vehicle Dynamic pedestrians\n",
        "    (244, 35, 232),  # Sidewalk Ground Terrain\n",
        "    (70, 130, 180),   # Water\n",
        "\n",
        "]) / 255.0 # normalize each channel [0-1] since is what Open3D uses"
      ],
      "metadata": {
        "id": "ac5xkgAQsQ3f"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [
        "ALPHA = 1\n",
        "BHETA = 1*np.pi/180\n",
        "RESOLUTION = 0.1\n",
        "MAX_RANGE = 70\n",
        "MAP_WIDTH = 100\n",
        "SPHERICAL2CARTESIAN_BIAS = 6\n",
        "### 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",
        "DYNAMIC_THRESHOLD=0.5\n",
        "num_classes=9\n",
        "FREE_CONF = 0.7\n",
        "OCC_CONF = 0.7\n",
        "DYNAMIC_THRESHOLD = 0.4\n",
        "sgm_time_0 = np.zeros((MAP_SIZE_Y,MAP_SIZE_X,num_classes))\n",
        "sgm_time_0[:,:,0] = 1"
      ],
      "metadata": {
        "id": "y1MH7VTN_BiY"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [
        "def load_matrix(folder_path,timestamp):\n",
        "  ts = str(timestamp)\n",
        "  for file_name in os.listdir(folder_path):\n",
        "          if os.path.splitext(file_name)[0] == ts:\n",
        "\n",
        "              file_path = os.path.join(folder_path,file_name)\n",
        "              #print(file_path)\n",
        "              break\n",
        "\n",
        "  m=np.load(file_path)\n",
        "  return m"
      ],
      "metadata": {
        "id": "oHNOrxJnep0G"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [
        "def filter_road_points(lidar_in, is_road, threshold=0.5):\n",
        "        y_true = lidar_in[:, 1]\n",
        "        y = np.zeros_like(y_true)\n",
        "        y = y_true * is_road  # set road points' predicted height to true height\n",
        "        delta_y = np.absolute(y_true-y).flatten()\n",
        "        is_not_road = delta_y > threshold\n",
        "        lidar_out = lidar_in[is_not_road, :].copy()\n",
        "        return lidar_out\n"
      ],
      "metadata": {
        "id": "jy49C-VvXbyQ"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [
        "L0_CONF = 0.1\n",
        "L1_CONF = 0.8\n",
        "L2_CONF = 1\n",
        "L3_CONF = 1\n",
        "L4_CONF = 1\n",
        "L5_CONF = 1\n",
        "L6_CONF = 1\n",
        "L7_CONF = 1\n",
        "L8_CONF = 1\n",
        "\n",
        "\n",
        "\n",
        "class_conf={\n",
        "    0 : L0_CONF,\n",
        "    1 : L1_CONF,\n",
        "    2 : L2_CONF,\n",
        "    3 : L3_CONF,\n",
        "    4 : L4_CONF,\n",
        "    5 : L5_CONF,\n",
        "    6 : L6_CONF,\n",
        "    7 : L7_CONF,\n",
        "    8 : L8_CONF}\n",
        "\n",
        "\n",
        "\n",
        "def generate_semantic_dgm(lidar_in, ogm_shape):\n",
        "  ### Calculate the position of LiDAR points in spherical coordinate\n",
        "    rphi_meas = np.zeros((lidar_in.shape[0], 3))\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[:, 2]=lidar_in[:,2]\n",
        "\n",
        "\n",
        "    rphi_meas = np.unique(rphi_meas.astype(int), axis=0)\n",
        "\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",
        "\n",
        "    label_grid_np = np.zeros((sg_ang_bin, sg_rng_bin,num_classes))\n",
        "    label_grid_np[:,:,0] = 1\n",
        "    label_grid = [[[] for _ in range(sg_rng_bin)] for _ in range(sg_ang_bin)]\n",
        "\n",
        "    # Condition 1\n",
        "\n",
        "    for i in range(rphi_meas.shape[0]):\n",
        "\n",
        "      ang = rphi_meas[i, 0]\n",
        "      rng = rphi_meas[i, 1]\n",
        "      label = rphi_meas[i, 2]\n",
        "      #label_grid[int(ang), int(rng)] = label\n",
        "      label_grid[ang][rng].append(label)\n",
        "\n",
        "    # Assign majority label to each cell\n",
        "    for ang in range(sg_ang_bin):\n",
        "        for rng in range(sg_rng_bin):\n",
        "            cell_labels = label_grid[ang][rng]\n",
        "\n",
        "            # Exclude label 0 from the list of cell_labels\n",
        "            #cell_labels = [label for label in cell_labels if label != 0]\n",
        "\n",
        "\n",
        "            if len(cell_labels) > 0:\n",
        "                # Count the occurrences of each label\n",
        "                  label_counts = Counter(cell_labels)\n",
        "                  label_counts = {label: label_counts[label] * class_conf.get(label, 1) for label in label_counts}\n",
        "                  total_count = sum(label_counts.values())\n",
        "\n",
        "                  # Calculate the appearance percentage for each label in cell_labels\n",
        "                  appearance_percentages = {label: count / total_count for label, count in label_counts.items()}\n",
        "\n",
        "                  # Fill in the remaining labels in label_counts with 0 counts\n",
        "                  for label in range(num_classes):\n",
        "                      if label not in label_counts:\n",
        "                          label_counts[label] = 0\n",
        "\n",
        "                  appearance_tuple = tuple(appearance_percentages.get(label, 0) for label in range(num_classes))\n",
        "                  #print(appearance_tuple)\n",
        "                  # Assign the appearance tuple to the grid cell\n",
        "                  label_grid_np[ang][rng] = appearance_tuple\n",
        "            else:\n",
        "\n",
        "              label_grid_np[ang][rng] = (1,0,0,0,0,0,0,0,0)\n",
        "    center = sg_ang_bin // 2\n",
        "    for ang in range(sg_ang_bin):\n",
        "        ang_arr = rphi_meas[rphi_meas[:, 0] == ang, 1]\n",
        "\n",
        "        if len(ang_arr) == 0:\n",
        "\n",
        "            label_grid_np[ang, :] = (0.3,0.7,0,0,0,0,0,0,0)\n",
        "        else:\n",
        "\n",
        "            min_r = np.min(ang_arr)\n",
        "\n",
        "            label_grid_np[ang, :(min_r)] = (0.3,0.7,0,0,0,0,0,0,0)\n",
        "\n",
        "            #label_grid_np[ang, :3] = (0.2, 0.6, 0, 0, 0, 0 ,0.2, 0, 0)\n",
        "\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",
        "\n",
        "    label_step = cv2.warpPolar(label_grid_np, ogm_sz, ogm_cen, radius, cv2.WARP_INVERSE_MAP)\n",
        "\n",
        "    label_step[OOR_MASK] = (1,0,0,0,0,0,0,0,0)\n",
        "    for i in range(label_step.shape[-1]):\n",
        "        label_step[:, :, i] = cv2.rotate(label_step[:, :, i], cv2.ROTATE_90_CLOCKWISE)\n",
        "    #label_step = cv2.rotate(label_step, cv2.ROTATE_90_CLOCKWISE)\n",
        "\n",
        "\n",
        "    return label_step"
      ],
      "metadata": {
        "id": "4HY7gN5W_hM_"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [
        "\n",
        "# Update the SGM with DST rule of combination\n",
        "def update_sgm(prior_dgm, new_dgm):\n",
        "    num_classes = new_dgm.shape[2]\n",
        "\n",
        "    conflict_mass = np.zeros_like(new_dgm[:, :, 0])\n",
        "    class_mass = np.zeros_like(new_dgm[:, :, 1:])\n",
        "\n",
        "\n",
        "\n",
        "    for i in range(1,num_classes):\n",
        "        for j in range(1,num_classes):\n",
        "\n",
        "            if i != j:\n",
        "                conflict_mass += np.multiply(prior_dgm[:, :, i], new_dgm[:, :, j])\n",
        "                conflict_mass += np.multiply(prior_dgm[:, :, j], new_dgm[:, :, i])\n",
        "\n",
        "        class_mass[:, :, i-1] += np.multiply(prior_dgm[:, :, 0], new_dgm[:, :, i])\n",
        "        class_mass[:, :, i-1] += np.multiply(prior_dgm[:, :, i], new_dgm[:, :, 0])\n",
        "        #class_mass[:, :, i-1] = np.divide(class_mass[:, :, i-1], np.ones_like(conflict_mass) - conflict_mass +1e-10)  #DS\n",
        "        class_mass[:, :, i-1] += conflict_mass #yager\n",
        "        #class_mass[:, :, i-1] += np.divide(class_mass[:, :, i-1], np.ones_like(conflict_mass) - conflict_mass +1e-10) #perso\n",
        "\n",
        "\n",
        "    #class0 is the unknown mass\n",
        "    unknown_mass = np.multiply(prior_dgm[:,:,0],new_dgm[:,:,0])\n",
        "    #unknown_mass = np.divide(unknown_mass,np.ones_like(conflict_mass) - conflict_mass+1e-10) #DS\n",
        "    unknown_mass += conflict_mass #yager\n",
        "    #unknown_mass = +np.divide(unknown_mass,np.ones_like(conflict_mass) - conflict_mass+1e-10) #perso\n",
        "    updated_dgm = np.dstack((unknown_mass,class_mass))\n",
        "\n",
        "    for i in range(prior_dgm.shape[0]):\n",
        "      for j in range(prior_dgm.shape[1]):\n",
        "          if np.array_equal(prior_dgm[i, j], new_dgm[i, j]):\n",
        "              # If the belief values at (i, j) are the same element-wise, assign the values to the masses\n",
        "              updated_dgm[i, j] = new_dgm[i, j]\n",
        "              continue\n",
        "\n",
        "\n",
        "    return updated_dgm, conflict_mass\n",
        "\n",
        "# Convert the SGM to a displayable figure\n",
        "def predict_sgm(dgm, dynamic_mass,DYNAMIC_THRESHOLD=0.4):\n",
        "\n",
        "    max_mass = np.argmax(dgm, axis=2)\n",
        "\n",
        "    pred_map = np.zeros(dgm[:,:,:3].shape)\n",
        "\n",
        "    #print(np.unique(max_mass))\n",
        "    for i in range(num_classes):\n",
        "        pred_map[max_mass == i] = REDUCED_LABEL_COLORS[i] * 255\n",
        "\n",
        "    # The dynamic cells:\n",
        "    pred_map[dynamic_mass >= DYNAMIC_THRESHOLD] = REDUCED_LABEL_COLORS[6] * 255\n",
        "\n",
        "    return pred_map.astype(np.uint8)\n"
      ],
      "metadata": {
        "id": "-4cSSagKYNge"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [
        "def update_sgm_scenario(*new_dgms):\n",
        "    num_classes = new_dgms[0].shape[2]\n",
        "    num_inputs = len(new_dgms)\n",
        "\n",
        "    conflict_mass = np.zeros_like(new_dgms[0][:, :, 0])\n",
        "    class_mass = np.zeros_like(new_dgms[0][:, :, 1:])\n",
        "    unknown_mass = np.zeros_like(new_dgms[0][:, :, 0])\n",
        "\n",
        "    for i in range(num_inputs - 1):\n",
        "        for j in range(i + 1, num_inputs):\n",
        "            for k in range(1, num_classes):\n",
        "                conflict_mass += np.multiply(new_dgms[i][:, :, k], new_dgms[j][:, :, k])\n",
        "                conflict_mass += np.multiply(new_dgms[j][:, :, k], new_dgms[i][:, :, k])\n",
        "\n",
        "            for k in range(1, num_classes):\n",
        "                class_mass[:, :, k-1] += np.multiply(new_dgms[i][:, :, 0], new_dgms[j][:, :, k])\n",
        "                class_mass[:, :, k-1] += np.multiply(new_dgms[i][:, :, k], new_dgms[j][:, :, 0])\n",
        "                #class_mass[:, :, k-1] = np.divide(class_mass[:, :, k-1], np.ones_like(conflict_mass) - conflict_mass + 1e-10)#DS\n",
        "                #class_mass[:, :, k-1] += conflict_mass #yager\n",
        "            unknown_mass *= np.multiply(unknown_mass, new_dgms[j][:, :, 0])\n",
        "            #unknown_mass = np.divide(unknown_mass,np.ones_like(conflict_mass) - conflict_mass+1e-10) #DS\n",
        "            #unknown_mass += conflict_mass #yager\n",
        "    updated_dgm = np.dstack((unknown_mass, class_mass))\n",
        "\n",
        "    for i in range(new_dgms[-1].shape[0]):\n",
        "        for j in range(new_dgms[-1].shape[1]):\n",
        "            if np.array_equal(new_dgms[-1][i, j], updated_dgm[i, j]):\n",
        "                updated_dgm[i, j] = new_dgms[-1][i, j]\n",
        "\n",
        "    return updated_dgm, conflict_mass"
      ],
      "metadata": {
        "id": "mkggadfgiv2w"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [
        "# Shift the map according the vehicle's poses\n",
        "def shift_pose_sgm(dgm, init, fin):\n",
        "  dgm_o = dgm.copy()\n",
        "  #theta = init[2] /180 * np.pi\n",
        "  theta = -init[2]\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",
        "\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]))\n",
        "  M = cv2.getRotationMatrix2D((dgm_o.shape[1]/2+0.5,dgm_o.shape[0]/2+0.5),-delta_theta* 180 / np.pi,1)\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]))\n",
        "\n",
        "  search_value = (0, 0, 0, 0, 0, 0, 0, 0, 0)\n",
        "  border_value = (1, 0, 0, 0, 0, 0, 0, 0, 0)\n",
        "\n",
        "  # Create a mask for pixels with the search value\n",
        "  mask = np.all(dst == search_value, axis=-1)\n",
        "\n",
        "  # Replace the pixels with the replace value\n",
        "  dst[mask] = border_value\n",
        "\n",
        "  return dst"
      ],
      "metadata": {
        "id": "cIFyHq9sYn2g"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [
        "vehicles=[]\n",
        "for item in os.listdir(os.path.join(datadir,scenario)):\n",
        "    # Construct the full path to the item\n",
        "    if os.path.isdir(item_path) and item != '.ipynb_checkpoints':\n",
        "        folder_path = os.path.join(os.path.join(datadir,scenario), item)\n",
        "        output_path = os.path.join(datadir,output)\n",
        "        lidar_path=folder_path+'/Lidar'\n",
        "        img_path=folder_path+'/bird_eye'\n",
        "        save_path=output_path+'SG fusion'\n",
        "        if not os.path.exists(save_path):\n",
        "            os.makedirs(save_path)\n",
        "        num_files = len([f for f in os.listdir(lidar_path) if os.path.isfile(os.path.join(lidar_path, f))])\n",
        "        print('Number of files in {} :'.format(lidar_path), num_files)\n",
        "        vehicles.append((folder_path,num_files,save_path))"
      ],
      "metadata": {
        "id": "C69xP8yQzhE7",
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "outputId": "e0a5eab2-44aa-4110-ace8-78073187bfe7"
      },
      "execution_count": null,
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "Number of files in /content/drive/MyDrive/s_data/ground truth/scenario_test2_gt/hero vehicle.lincoln.mkz_2017/Lidar : 406\n",
            "Number of files in /content/drive/MyDrive/s_data/ground truth/scenario_test2_gt/adversary vehicle.tesla.model3/Lidar : 406\n"
          ]
        }
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "vehicles"
      ],
      "metadata": {
        "id": "7yYMhfzbfEBI",
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "outputId": "22a5a420-76cf-48de-b71b-96023cbd70c6"
      },
      "execution_count": null,
      "outputs": [
        {
          "output_type": "execute_result",
          "data": {
            "text/plain": [
              "[('/content/drive/MyDrive/s_data/ground truth/scenario_test2_gt/hero vehicle.lincoln.mkz_2017',\n",
              "  406,\n",
              "  '/content/drive/MyDrive/s_data/output/scenario_test2_gt/SG fusion'),\n",
              " ('/content/drive/MyDrive/s_data/ground truth/scenario_test2_gt/adversary vehicle.tesla.model3',\n",
              "  406,\n",
              "  '/content/drive/MyDrive/s_data/output/scenario_test2_gt/SG fusion')]"
            ]
          },
          "metadata": {},
          "execution_count": 12
        }
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "folder_paths=[]\n",
        "\n",
        "for i, vehicle in enumerate(vehicles, start=1):\n",
        "    variable_name = f'folder_path{i}'\n",
        "    folder_path = os.path.join(datadir, scenario, vehicle[0])\n",
        "    globals()[variable_name] = folder_path\n",
        "    folder_paths.append(folder_path)\n",
        "nbr_frames=[]\n",
        "for i, vehicle in enumerate(vehicles, start=1):\n",
        "\n",
        "    nbr = vehicle[1]\n",
        "\n",
        "    nbr_frames.append(nbr)\n",
        "nbr=min(nbr_frames)"
      ],
      "metadata": {
        "id": "yoQtgED-gFFE"
      },
      "execution_count": null,
      "outputs": []
    },
    {
      "cell_type": "code",
      "source": [
        "folder_paths"
      ],
      "metadata": {
        "id": "4GpP8hjFgI-a",
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "outputId": "bff63208-6fbe-4ed8-d72e-1f723ad62315"
      },
      "execution_count": null,
      "outputs": [
        {
          "output_type": "execute_result",
          "data": {
            "text/plain": [
              "['/content/drive/MyDrive/s_data/ground truth/scenario_test2_gt/hero vehicle.lincoln.mkz_2017',\n",
              " '/content/drive/MyDrive/s_data/ground truth/scenario_test2_gt/adversary vehicle.tesla.model3']"
            ]
          },
          "metadata": {},
          "execution_count": 14
        }
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "'''\n",
        "import random\n",
        "seed_value = 13\n",
        "\n",
        "# Set the seed for the random number generator\n",
        "random.seed(seed_value)\n",
        "# Randomly permute the order of folder_paths\n",
        "random.shuffle(folder_paths)\n",
        "print(folder_paths)'''"
      ],
      "metadata": {
        "id": "b2_uOBTkRHPM",
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 36
        },
        "outputId": "741e1744-cc95-4362-98df-648f7ec08ea2"
      },
      "execution_count": null,
      "outputs": [
        {
          "output_type": "execute_result",
          "data": {
            "text/plain": [
              "'\\nimport random\\nseed_value = 13\\n\\n# Set the seed for the random number generator\\nrandom.seed(seed_value)\\n# Randomly permute the order of folder_paths\\nrandom.shuffle(folder_paths)\\nprint(folder_paths)'"
            ],
            "application/vnd.google.colaboratory.intrinsic+json": {
              "type": "string"
            }
          },
          "metadata": {},
          "execution_count": 15
        }
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "folder_paths"
      ],
      "metadata": {
        "id": "uOXVwob1tyk8",
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "outputId": "6810d9d0-6a61-4600-94e7-d1e7ad330ca0"
      },
      "execution_count": null,
      "outputs": [
        {
          "output_type": "execute_result",
          "data": {
            "text/plain": [
              "['/content/drive/MyDrive/s_data/ground truth/scenario_test2_gt/hero vehicle.lincoln.mkz_2017',\n",
              " '/content/drive/MyDrive/s_data/ground truth/scenario_test2_gt/adversary vehicle.tesla.model3']"
            ]
          },
          "metadata": {},
          "execution_count": 16
        }
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "print(nbr)"
      ],
      "metadata": {
        "id": "jf3IhCG6xCtS",
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "outputId": "f875d6a3-622d-4674-e2f0-73d7426e9444"
      },
      "execution_count": null,
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "406\n"
          ]
        }
      ]
    },
    {
      "cell_type": "code",
      "source": [
        "for date in range(nbr):\n",
        "\n",
        "  for i, folder_path in enumerate(folder_paths, start=1):\n",
        "\n",
        "      save_path=vehicles[i-1][2]+'/fusion_image'\n",
        "      save_path1=vehicles[i-1][2]+'/mass_function'\n",
        "      save_path2=vehicles[i-1][2]+'/conflict'\n",
        "      if not os.path.exists(save_path):\n",
        "        os.makedirs(save_path)\n",
        "      if not os.path.exists(save_path1):\n",
        "        os.makedirs(save_path1)\n",
        "      if not os.path.exists(save_path2):\n",
        "        os.makedirs(save_path2)\n",
        "      transformation_matrices_path = os.path.join(folder_path, \"pos_matrices/lidar_2_world\")\n",
        "      transformation_matrices_path_variable = f'transformation_matrices_path{i}'\n",
        "      globals()[transformation_matrices_path_variable] = transformation_matrices_path\n",
        "\n",
        "\n",
        "\n",
        "      transformation_matrix_variable = f'transform{i}'\n",
        "      globals()[transformation_matrix_variable] = load_matrix(transformation_matrices_path, date)\n",
        "      transform = globals()[transformation_matrix_variable]\n",
        "\n",
        "\n",
        "      location_variable = f'loc{i}'\n",
        "      globals()[location_variable] = transform[:3, 3:].flatten().tolist()\n",
        "\n",
        "      rotation_path = os.path.join(folder_path, \"pos_matrices/rotation_pyr\")\n",
        "      rotation_path_variable = f'rot_path{i}'\n",
        "      globals()[rotation_path_variable] = rotation_path\n",
        "\n",
        "      rotation_variable = f'rot{i}'\n",
        "      globals()[rotation_variable] = load_matrix(rotation_path, date)\n",
        "\n",
        "\n",
        "\n",
        "      R = np.array(transform[:3, :3])\n",
        "      pitch = np.arcsin(R[2, 0])\n",
        "      yaw = -np.arctan2(R[1, 0], R[0, 0])\n",
        "      roll = -np.arctan2(R[2, 1], R[2, 2])\n",
        "      pose_variable = f'pose{i}'\n",
        "      globals()[pose_variable] = np.array([globals()[location_variable][0], globals()[location_variable][1], yaw])\n",
        "\n",
        "\n",
        "      lidar_variable = f'lidar{i}'\n",
        "      lidar_path = os.path.join(folder_path, \"Lidar\")\n",
        "      lidar_file = os.path.join(lidar_path, f'{date}.npy')\n",
        "\n",
        "      lidar_data = np.load(lidar_file)\n",
        "      globals()[lidar_variable] = lidar_data.reshape(-1, 6)\n",
        "\n",
        "      lidar_raw_variable = f'lidar_raw{i}'\n",
        "      globals()[lidar_raw_variable] = globals()[lidar_variable].reshape(-1, 6)\n",
        "\n",
        "      globals()[lidar_raw_variable] = globals()[lidar_raw_variable][globals()[lidar_raw_variable][:, 2] <= 0, :]\n",
        "\n",
        "      radius = 2.7\n",
        "      distances_variable = f'distances{i}'\n",
        "      distances = np.sqrt((globals()[lidar_raw_variable][:, 0])**2 + (globals()[lidar_raw_variable][:, 1])**2)\n",
        "      globals()[distances_variable] = distances\n",
        "\n",
        "      globals()[lidar_raw_variable] = globals()[lidar_raw_variable][globals()[distances_variable] > radius, :]\n",
        "      labels_variable = f'labels{i}'\n",
        "      labels = np.array(globals()[lidar_raw_variable][:, 5]).astype(int)\n",
        "      globals()[labels_variable] = labels\n",
        "\n",
        "      lidar_labeled_variable = f'lidar_labeled{i}'\n",
        "      lidar_labeled = np.array(globals()[lidar_raw_variable][..., [0, 1, 2, 5]])\n",
        "      globals()[lidar_labeled_variable] = lidar_labeled\n",
        "\n",
        "      globals()[lidar_raw_variable] = globals()[lidar_raw_variable][:, :3]\n",
        "\n",
        "      road_filter_variable = f'road_filter{i}'\n",
        "      road_filter = np.zeros_like(globals()[labels_variable])\n",
        "      road_filter[(globals()[labels_variable] == 0) | (globals()[labels_variable] == 7) | (globals()[labels_variable] == 6)] = 1\n",
        "      globals()[road_filter_variable] = road_filter\n",
        "\n",
        "      lidar_nonroad_variable = f'lidar_nonroad{i}'\n",
        "      globals()[lidar_nonroad_variable] = filter_road_points(globals()[lidar_raw_variable], globals()[road_filter_variable], 0.1)\n",
        "\n",
        "\n",
        "      reduced_labels_variable = f'reduced_labels{i}'\n",
        "\n",
        "      lidar_rlabeled_variable = f'lidar_rlabeled{i}'\n",
        "      lidar_nonroad_rlabeled_variable = f'lidar_nonroad_rlabeled{i}'\n",
        "\n",
        "\n",
        "      globals()[reduced_labels_variable] = np.zeros_like(globals()[labels_variable])\n",
        "      globals()[reduced_labels_variable][(globals()[labels_variable] == 7) | (globals()[labels_variable] == 6)] = 1\n",
        "      globals()[reduced_labels_variable][(globals()[labels_variable] == 1) | (globals()[labels_variable] == 2) | (globals()[labels_variable] == 11) | (globals()[labels_variable] == 19)] = 2\n",
        "      globals()[reduced_labels_variable][(globals()[labels_variable] == 3) | (globals()[labels_variable] == 15) | (globals()[labels_variable] == 17) | (globals()[labels_variable] == 18)] = 3\n",
        "      globals()[reduced_labels_variable][(globals()[labels_variable] == 5) | (globals()[labels_variable] == 18) | (globals()[labels_variable] == 19)] = 4\n",
        "      globals()[reduced_labels_variable][(globals()[labels_variable] == 9)] = 5\n",
        "      globals()[reduced_labels_variable][(globals()[labels_variable] == 4) | (globals()[labels_variable] == 10) | (globals()[labels_variable] == 20)] = 6\n",
        "      globals()[reduced_labels_variable][(globals()[labels_variable] == 8) | (globals()[labels_variable] == 14) | (globals()[labels_variable] == 22)] = 7\n",
        "      globals()[reduced_labels_variable][(globals()[labels_variable] == 21)] = 8\n",
        "\n",
        "\n",
        "      globals()[lidar_rlabeled_variable] = np.concatenate((globals()[lidar_raw_variable], globals()[reduced_labels_variable][..., np.newaxis]), axis=-1)\n",
        "      road_filter_variable = f'road_filter{i}'\n",
        "      globals()[lidar_nonroad_rlabeled_variable] = filter_road_points(globals()[lidar_rlabeled_variable], globals()[road_filter_variable], 0.1)\n",
        "\n",
        "\n",
        "      lidar_sgm_variable = f'lidar_sgm{i}'\n",
        "\n",
        "      lidar_nonroad_rlabeled = globals()[lidar_nonroad_rlabeled_variable]\n",
        "      globals()[lidar_sgm_variable] = lidar_nonroad_rlabeled[:, [0, 1, 3]]\n",
        "\n",
        "      sgm_variable = f'sgm{i}'\n",
        "\n",
        "      globals()[sgm_variable] = generate_semantic_dgm(globals()[lidar_sgm_variable], sgm_time_0.shape)\n",
        "\n",
        "\n",
        "      sgm_time_variable = f'sgm_time_{i}'\n",
        "      dynamic_mass_variable = f'dynamic_mass_{i}'\n",
        "      sgm_pred_variable = f'sgm_pred_{i}'\n",
        "\n",
        "      globals()[sgm_time_variable], globals()[dynamic_mass_variable] = update_sgm(sgm_time_0, globals()[sgm_variable])\n",
        "      globals()[sgm_pred_variable] = predict_sgm(globals()[sgm_time_variable], globals()[dynamic_mass_variable],0.7)\n",
        "\n",
        "  shift_sgm_time = [shift_pose_sgm(sgm_time_1, pose1, pose2)]\n",
        "\n",
        "  if 'pose3' in globals():\n",
        "      shift_sgm_time.append(shift_pose_sgm(sgm_time_3, pose3, pose2))\n",
        "\n",
        "  if 'pose4' in globals():\n",
        "      shift_sgm_time.append(shift_pose_sgm(sgm_time_4, pose4, pose2))\n",
        "\n",
        "  sgm_fuse, dynamic_mass_fuse = update_sgm_scenario(sgm_time_2, *shift_sgm_time)\n",
        "  sgm_pred_fuse = predict_sgm(sgm_fuse, dynamic_mass_fuse, 100)\n",
        "  '''plt.imshow(sgm_pred_fuse)\n",
        "  plt.show()'''\n",
        "  rgb_image = cv2.cvtColor(sgm_pred_fuse, cv2.COLOR_BGR2RGB)\n",
        "  cv2.imwrite(save_path+'/{}.png'.format(date), rgb_image)\n",
        "  #imageio.imwrite(save_path+'/{}.jpg'.format(date),sgm_pred_fuse)\n",
        "  print(save_path+'/{}.png'.format(date))\n",
        "  np.save(save_path1+'/{}.npy'.format(date),sgm_fuse)\n",
        "  print(save_path1+'/{}.npy'.format(date))\n",
        "  np.save(save_path2+'/{}.npy'.format(date),dynamic_mass_fuse)\n",
        "  print(save_path2+'/{}.npy'.format(date))"
      ],
      "metadata": {
        "id": "-SFxpDRsXfdn",
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "outputId": "67a6a936-037b-4dd5-dd45-28101c82f43d"
      },
      "execution_count": null,
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "/content/drive/MyDrive/s_data/output/scenario_test2_gt/SG fusion/fusion_image/0.png\n",
            "/content/drive/MyDrive/s_data/output/scenario_test2_gt/SG fusion/mass_function/0.npy\n",
            "/content/drive/MyDrive/s_data/output/scenario_test2_gt/SG fusion/conflict/0.npy\n"
          ]
        }
      ]
    }
  ],
  "metadata": {
    "colab": {
      "provenance": [],
      "include_colab_link": true
    },
    "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
}