Semantic_grid_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/Semantic_grid_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": 1,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "FBjbCBkk6gI1",
        "outputId": "9d71b44a-193b-4267-e73f-65172d66e4b3"
      },
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "Mounted at /content/drive\n"
          ]
        }
      ],
      "source": [
        "import os\n",
        "import sys\n",
        "import csv\n",
        "import numpy as np\n",
        "import cv2\n",
        "import matplotlib.pyplot as plt\n",
        "import time\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",
      "execution_count": 2,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "gu1FbSfH4R6f",
        "outputId": "11a4447a-67d1-4b68-e741-660876336f79"
      },
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_2 vehicle.mini.cooper_s\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Ego_1 vehicle.audi.a2\n"
          ]
        }
      ],
      "source": [
        "datadir = '/content/drive/MyDrive/s_data/'\n",
        "scenario='ground truth/scenario7_gt/'\n",
        "\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"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "C69xP8yQzhE7",
        "outputId": "5c182f5c-c410-415d-c8f3-2d13de593f47"
      },
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "Number of files in /content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/Lidar : 122\n",
            "Number of files in /content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/Lidar : 121\n",
            "Number of files in /content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_2 vehicle.mini.cooper_s/Lidar : 121\n",
            "Number of files in /content/drive/MyDrive/s_data/ground truth/scenario7_gt/Ego_1 vehicle.audi.a2/Lidar : 122\n"
          ]
        }
      ],
      "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",
        "        lidar_path=folder_path+'/Lidar'\n",
        "        img_path=folder_path+'/bird_eye'\n",
        "        save_path=folder_path+'/sem_grid_gt'\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((lidar_path,num_files,save_path))"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/"
        },
        "id": "aB0lOZT446Yn",
        "outputId": "534f1526-64dd-4513-dc31-3e39c376d2cf"
      },
      "outputs": [
        {
          "output_type": "execute_result",
          "data": {
            "text/plain": [
              "[('/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/Lidar',\n",
              "  122,\n",
              "  '/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt'),\n",
              " ('/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/Lidar',\n",
              "  121,\n",
              "  '/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt'),\n",
              " ('/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_2 vehicle.mini.cooper_s/Lidar',\n",
              "  121,\n",
              "  '/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_2 vehicle.mini.cooper_s/sem_grid_gt'),\n",
              " ('/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Ego_1 vehicle.audi.a2/Lidar',\n",
              "  122,\n",
              "  '/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Ego_1 vehicle.audi.a2/sem_grid_gt')]"
            ]
          },
          "metadata": {},
          "execution_count": 4
        }
      ],
      "source": [
        "vehicles"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "id": "y1MH7VTN_BiY"
      },
      "outputs": [],
      "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"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 6,
      "metadata": {
        "id": "OcM0SMNa-2qx"
      },
      "outputs": [],
      "source": [
        "def filter_road_points(lidar_in, is_road, threshold=0.5):\n",
        "  y_true = lidar_in[:,1]\n",
        "  y_pred = np.zeros_like(y_true)\n",
        "\n",
        "  y_pred = y_true * is_road  # set road points' predicted height to true height\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"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 7,
      "metadata": {
        "id": "4HY7gN5W_hM_"
      },
      "outputs": [],
      "source": [
        "from collections import Counter\n",
        "\n",
        "label_weights = {\n",
        "    0: 0.1,\n",
        "    1: 0.2,\n",
        "    2: 1,\n",
        "    3: 1 ,\n",
        "    4: 1,\n",
        "    5: 1,\n",
        "    6: 1,\n",
        "    7: 1,\n",
        "    8: 1\n",
        "}\n",
        "\n",
        "\n",
        "def generate_semantic_ogm(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",
        "    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",
        "\n",
        "    scan_grid = np.ones((sg_ang_bin, sg_rng_bin)) * 0.5\n",
        "    label_grid_np = np.zeros((sg_ang_bin, sg_rng_bin))\n",
        "    label_grid = [[[] for _ in range(sg_rng_bin)] for _ in range(sg_ang_bin)]\n",
        "    # Condition 1\n",
        "\n",
        "    scan_grid[tuple(rphi_meas[:,[0,1]].T)] = 0.7\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",
        "\n",
        "    # Condition 2\n",
        "\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",
        "            if len(cell_labels) > 0:\n",
        "                  # Count the occurrences of each label\n",
        "                  label_counts = Counter(cell_labels)\n",
        "\n",
        "                  # Calculate the weighted counts\n",
        "                  weighted_counts = {label: label_counts[label] * label_weights.get(label, 1) for label in label_counts}\n",
        "\n",
        "                  # Get the label with the maximum weighted count\n",
        "                  majority_label = max(weighted_counts, key=weighted_counts.get)\n",
        "\n",
        "                  # Assign the majority label to the grid cell\n",
        "                  label_grid_np[ang][rng] = majority_label\n",
        "            else:\n",
        "\n",
        "              label_grid_np[ang][rng] = 0\n",
        "\n",
        "\n",
        "        ang_arr = rphi_meas[rphi_meas[:, 0] == ang, 1]\n",
        "\n",
        "        if len(ang_arr) == 0:\n",
        "            scan_grid[ang, :] = 0.3\n",
        "            label_grid_np[ang, :] = 1\n",
        "        else:\n",
        "\n",
        "            min_r = np.min(ang_arr)\n",
        "            scan_grid[ang, :min_r] = 0.3\n",
        "            label_grid_np[ang, :(min_r)] = 1\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",
        "    label_step = cv2.warpPolar(label_grid_np, ogm_sz, ogm_cen, radius, cv2.WARP_INVERSE_MAP)\n",
        "    ogm_step[OOR_MASK] = 0.5\n",
        "    label_step[OOR_MASK] = 0\n",
        "    ogm_step = cv2.rotate(ogm_step, cv2.ROTATE_90_CLOCKWISE)\n",
        "    label_step = cv2.rotate(label_step, cv2.ROTATE_90_CLOCKWISE)\n",
        "    ogm_step = np.dstack([ogm_step, label_step])\n",
        "\n",
        "    return ogm_step"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 8,
      "metadata": {
        "colab": {
          "base_uri": "https://localhost:8080/",
          "height": 1000
        },
        "id": "RdI9iz9X6QJy",
        "outputId": "b319611d-ec1e-46f5-d222-80676e4b70b6"
      },
      "outputs": [
        {
          "output_type": "stream",
          "name": "stdout",
          "text": [
            "('/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/Lidar', 122, '/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt')\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/0.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/1.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/2.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/3.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/4.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/5.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/6.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/7.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/8.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/9.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/10.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/11.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/12.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/13.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/14.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/15.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/16.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/17.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/18.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/19.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/20.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/21.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/22.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/23.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/24.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/25.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/26.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/27.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/28.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/29.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/30.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/31.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/32.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/33.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/34.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/35.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/36.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/37.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/38.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/39.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/40.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/41.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/42.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/43.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/44.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/45.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/46.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/47.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/48.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/49.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/50.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/51.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/52.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/53.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/54.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/55.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/56.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/57.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/58.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/59.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/60.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/61.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/62.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/63.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/64.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/65.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/66.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/67.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/68.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/69.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/70.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/71.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/72.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/73.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/74.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/75.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/76.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/77.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/78.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/79.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/80.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/81.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/82.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/83.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/84.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/85.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/86.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/87.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/88.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/89.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/90.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/91.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/92.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/93.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/94.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/95.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/96.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/97.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/98.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/99.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/100.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/101.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/102.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/103.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/104.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/105.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/106.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/107.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/108.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/109.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/110.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/111.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/112.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/113.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/114.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/115.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/116.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/117.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/118.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/119.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/120.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_3 vehicle.nissan.patrol/sem_grid_gt/121.npy\n",
            "('/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/Lidar', 121, '/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt')\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/0.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/1.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/2.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/3.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/4.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/5.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/6.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/7.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/8.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/9.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/10.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/11.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/12.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/13.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/14.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/15.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/16.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/17.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/18.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/19.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/20.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/21.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/22.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/23.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/24.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/25.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/26.npy\n",
            "/content/drive/MyDrive/s_data/ground truth/scenario7_gt/Vehicle_1 vehicle.nissan.micra/sem_grid_gt/27.npy\n"
          ]
        },
        {
          "output_type": "error",
          "ename": "KeyboardInterrupt",
          "evalue": "ignored",
          "traceback": [
            "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
            "\u001b[0;31mKeyboardInterrupt\u001b[0m                         Traceback (most recent call last)",
            "\u001b[0;32m<ipython-input-8-bb0637d7a17f>\u001b[0m in \u001b[0;36m<cell line: 1>\u001b[0;34m()\u001b[0m\n\u001b[1;32m     15\u001b[0m             \u001b[0mall_files_exist\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;32mFalse\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     16\u001b[0m             \u001b[0;32mcontinue\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 17\u001b[0;31m     \u001b[0mdata\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mnp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mload\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mlidar_path\u001b[0m\u001b[0;34m+\u001b[0m\u001b[0;34m'/{}.npy'\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mformat\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mi\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m     18\u001b[0m     \u001b[0mlidar_raw\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mdata\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mreshape\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m6\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     19\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n",
            "\u001b[0;32m/usr/local/lib/python3.10/dist-packages/numpy/lib/npyio.py\u001b[0m in \u001b[0;36mload\u001b[0;34m(file, mmap_mode, allow_pickle, fix_imports, encoding)\u001b[0m\n\u001b[1;32m    428\u001b[0m                 \u001b[0;32mreturn\u001b[0m \u001b[0mformat\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mopen_memmap\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mfile\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mmode\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mmmap_mode\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    429\u001b[0m             \u001b[0;32melse\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 430\u001b[0;31m                 return format.read_array(fid, allow_pickle=allow_pickle,\n\u001b[0m\u001b[1;32m    431\u001b[0m                                          pickle_kwargs=pickle_kwargs)\n\u001b[1;32m    432\u001b[0m         \u001b[0;32melse\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
            "\u001b[0;32m/usr/local/lib/python3.10/dist-packages/numpy/lib/format.py\u001b[0m in \u001b[0;36mread_array\u001b[0;34m(fp, allow_pickle, pickle_kwargs)\u001b[0m\n\u001b[1;32m    754\u001b[0m         \u001b[0;32mif\u001b[0m \u001b[0misfileobj\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mfp\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    755\u001b[0m             \u001b[0;31m# We can use the fast fromfile() function.\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m--> 756\u001b[0;31m             \u001b[0marray\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mnumpy\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mfromfile\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mfp\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mdtype\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mdtype\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mcount\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mcount\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m    757\u001b[0m         \u001b[0;32melse\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m    758\u001b[0m             \u001b[0;31m# This is not a real file. We have to read it the\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
            "\u001b[0;31mKeyboardInterrupt\u001b[0m: "
          ]
        }
      ],
      "source": [
        "for item in vehicles:\n",
        "  print(item)\n",
        "  for  i in range(item[1]):\n",
        "    lidar_path=item[0]\n",
        "    save_path=item[2]\n",
        "    if not os.path.exists(save_path):\n",
        "      os.makedirs(save_path)\n",
        "\n",
        "    '''if  os.path.exists(save_path+'/{}.npy'.format(i)):\n",
        "            print(\"File {} is already created. Skipping item...\".format(i))\n",
        "\n",
        "            continue'''\n",
        "    if not os.path.exists(lidar_path+'/{}.npy'.format(i)):\n",
        "            print(\"File {} not found. Skipping item...\".format(i))\n",
        "            all_files_exist = False\n",
        "            continue\n",
        "    data=np.load(lidar_path+'/{}.npy'.format(i))\n",
        "    lidar_raw=data.reshape(-1, 6)\n",
        "\n",
        "\n",
        "    lidar_raw = lidar_raw[lidar_raw[:, 2] <= 0, :]\n",
        "\n",
        "\n",
        "    # Define the radius (in meters)\n",
        "    radius = 3\n",
        "\n",
        "    # Calculate the Euclidean distance between each point in the point cloud and the lidar sensor position\n",
        "    distances = np.sqrt((lidar_raw[:, 0])**2 + (lidar_raw[:, 1])**2)\n",
        "\n",
        "    # Filter out all the points that are within the radius of the lidar sensor position\n",
        "    lidar_raw = lidar_raw[distances > radius, :]\n",
        "\n",
        "\n",
        "\n",
        "    intensity = np.array(lidar_raw[:, 3])\n",
        "    labels=np.array(lidar_raw[:, 5]).astype(int)\n",
        "\n",
        "    lidar_labeled=np.array(lidar_raw[..., [0, 1, 2, 5]])\n",
        "\n",
        "    lidar_raw = lidar_raw[:, :3]\n",
        "\n",
        "    road_filter=np.zeros_like(labels)\n",
        "    road_filter[(labels == 0) | (labels == 7) | (labels == 6)] = 1\n",
        "\n",
        "    reduced_labels=np.zeros_like(labels)\n",
        "    reduced_labels[(labels == 7) | (labels == 6)] = 1\n",
        "    reduced_labels[(labels == 1)| (labels == 2) | (labels == 11) | (labels == 19)] = 2\n",
        "    reduced_labels[(labels == 3)| (labels == 15) | (labels == 17) | (labels == 18)] = 3\n",
        "    reduced_labels[(labels == 5)| (labels == 18) | (labels == 19)] = 4\n",
        "    reduced_labels[(labels == 9)] = 5\n",
        "    reduced_labels[(labels == 4)| (labels == 10) | (labels == 20)] = 6\n",
        "    reduced_labels[(labels == 8)| (labels == 14) | (labels == 22)] = 7\n",
        "    reduced_labels[(labels == 21)] = 8\n",
        "\n",
        "\n",
        "\n",
        "    lidar_rlabeled = np.concatenate((lidar_raw, reduced_labels[..., np.newaxis]), axis=-1)\n",
        "\n",
        "    lidar_nonroad_labeled = filter_road_points(lidar_rlabeled,road_filter,0.1)\n",
        "\n",
        "\n",
        "    ### Initialize OGM\n",
        "    ogm_time_0 = np.ones((MAP_SIZE_Y,MAP_SIZE_X,2))\n",
        "\n",
        "    ### Only use the x-z axis of the point (ignore the height axis)\n",
        "    lidar_ogm = lidar_nonroad_labeled[:,[0,1,3]]\n",
        "\n",
        "\n",
        "    ogm_step = generate_semantic_ogm(lidar_ogm,ogm_time_0.shape)\n",
        "    np.save(save_path+'/{}.npy'.format(i),ogm_step)\n",
        "    print(save_path+'/{}.npy'.format(i))\n",
        "    '''### Visualize\n",
        "    fig,axs = plt.subplots(figsize=(12,6))\n",
        "    plt.imshow(((1-ogm_step[:, :, 0])*255).astype(np.uint8),cmap='gray')\n",
        "    plt.show()'''"
      ]
    }
  ],
  "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
}