detect.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The autoreload extension is already loaded. To reload it, use:\n",
      "  %reload_ext autoreload\n"
     ]
    }
   ],
   "source": [
    "#after running this, no need to restart the kurnel when you modify other files.\n",
    "%load_ext autoreload\n",
    "%autoreload 2"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "File loading and Set a view point"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "import open3d\n",
    "from settings import DATA_PATH\n",
    "import clusterization.clusterize as clusterizer\n",
    "import numpy as np\n",
    "from utils.visualizer import save_camera_params\n",
    "from utils.handling_cluster import visualize_by_cluster, crop_cluster\n",
    "from detection.detect import compute_nearest_cluster, change_cluster_color\n",
    "import utils.visualizer as visualizer\n",
    "\n",
    "data_type_name_pcd = \"LIVOX_Hallway_pcds\"\n",
    "experiment_name = \"Walking_to_end_1st1\"\n",
    "dir_name = \"res100ms_start30s\"\n",
    "\n",
    "#directory path of pcd files\n",
    "directory_path = os.path.join(DATA_PATH, data_type_name_pcd, experiment_name, dir_name)\n",
    "#'{DATA_PATH}/LIVOX_Hallway_pcds/jogging_fast_4th/res100ms_start13.5s'\n",
    "\n",
    "#add save_path\n",
    "experiment_name_clustered = experiment_name + \"_clustered\"\n",
    "dir_name_clustered= dir_name + \"_clustered\"\n",
    "folder_clustered = os.path.join(DATA_PATH, data_type_name_pcd, experiment_name_clustered, dir_name_clustered)\n",
    "\n",
    "if not os.path.exists(folder_clustered):\n",
    "    os.makedirs(folder_clustered)\n",
    "\n",
    "# make the list of files\n",
    "file_list = []\n",
    "for filename in os.listdir(directory_path):\n",
    "    if os.path.isfile(os.path.join(directory_path, filename)):\n",
    "        file_list.append(os.path.join(directory_path, filename))\n",
    "        \n",
    "#select your view point and close your window.\n",
    "pcd = open3d.io.read_point_cloud(file_list[0])\n",
    "cam_params = save_camera_params(pcd)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Detection"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 14\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.03 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 10\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 17\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 15\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 10\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 15\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 14\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.03 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.03 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 10\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 10\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 10\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 15\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 16\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 10\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 14\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 10\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.03 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 14\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 10\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 14\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 10\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 10\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 9\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 14\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 14\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 10\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 10\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 10\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 9\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 10\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 14\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 9\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 10\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 9\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 14\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 15\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 13\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 12\n",
      "Plane equation: -0.00x + -0.02y + 1.00z + 1.04 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.29 = 0\n",
      "num_clusters 11\n",
      "Plane equation: -0.00x + -0.01y + 1.00z + 1.03 = 0\n",
      "Plane equation: 0.00x + -0.02y + 1.00z + -2.28 = 0\n",
      "num_clusters 10\n"
     ]
    }
   ],
   "source": [
    "#add save_path\n",
    "experiment_name_tracked = experiment_name + \"_tracked\"\n",
    "dir_name_tracked= dir_name + \"_tracked\"\n",
    "folder_tracked = os.path.join(DATA_PATH, data_type_name_pcd, experiment_name_tracked, dir_name_tracked)\n",
    "\n",
    "if not os.path.exists(folder_tracked):\n",
    "    os.makedirs(folder_tracked)\n",
    "\n",
    "#initialization\n",
    "eps = 0.5\n",
    "center_prev = np.zeros(3) #initialize center of human cluster in previous frame\n",
    "\n",
    "#clusterize each files and visualize.\n",
    "for i, pcd_file in enumerate(file_list):\n",
    "    pcd = open3d.io.read_point_cloud(pcd_file)\n",
    "    \n",
    "    pcd_clustered = folder_clustered + \"/\" + os.path.basename(pcd_file) #pcd_clustered_save_path   \n",
    "    \n",
    "    #clusterization\n",
    "    if i==0:\n",
    "        visualize = True\n",
    "    else:\n",
    "        visualize = False\n",
    "    pcd, labels = clusterizer.clusterize(pcd, eps=eps,\n",
    "                             time_duration=None, #time duration that each windows open for. If you choose None, close window manually.\n",
    "                             window_name=os.path.basename(pcd_file), #window name changes accorrding to file name.\n",
    "                             cam_params=cam_params,#camera parameter(view point) you selected\n",
    "                             save_path=pcd_clustered,\n",
    "                             visualize=visualize\n",
    "                             ) \n",
    "    \n",
    "    #select human cluster in first frame\n",
    "    #select cluster id in comand line. check each window name.\n",
    "    if i==0:\n",
    "        visualize_by_cluster(pcd, labels)\n",
    "        cluster_id = int(input(\"Please enter cluster id: \"))\n",
    "    else:\n",
    "        cluster_id = compute_nearest_cluster(pcd, labels, center_prev)\n",
    "        \n",
    "    #color of points in cluster_id is changed into red.\n",
    "    pcd = change_cluster_color(pcd, labels, cluster_id=cluster_id)\n",
    "    visualizer.visualize(pcd, time_duration=0.5, cam_params=cam_params)\n",
    "    \n",
    "    #crop (and save) selected cluser\n",
    "    cropped_pcd = crop_cluster(pcd, labels, cluster_id=cluster_id, visualize=False)\n",
    "    \n",
    "    #compute center and distance between two centers\n",
    "    center_tmp = cropped_pcd.get_center()\n",
    "    current_dist = np.linalg.norm (center_prev - center_tmp)\n",
    "    \n",
    "    #update center if current cluster is close to previous one\"\n",
    "    if current_dist < 3:\n",
    "        center_prev = center_tmp\n",
    "    \n",
    "    #update center if current cluster is close to previous one\n",
    "    center_prev = center_tmp\n",
    "    \n",
    "    \n",
    "    # save the cropped cluster(pcd, save_path)\n",
    "    save_path_tracked = folder_tracked + '/' + os.path.basename(pcd_file)\n",
    "    if save_path_tracked != None:\n",
    "        open3d.io.write_point_cloud(save_path_tracked, cropped_pcd)\n",
    "    \n",
    "\n",
    "#label_list could be used later."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([1.82205592, 0.28191469, 0.10410521])"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "center_prev"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "lidar_soccer",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.11"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}