trap/test_tracking_data.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Visualise tracking data\n",
    "\n",
    "This notebook is an adapted version of `process.py` as it is included in the Trajectron++ package. It can be used to \n",
    "\n",
    "* Visualise the recorded trajectories, which are normally parsed to a Trajectron++ Node"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/ruben/suspicion/trap/.venv/lib/python3.10/site-packages/tqdm/auto.py:21: TqdmWarning: IProgress not found. Please update jupyter and ipywidgets. See https://ipywidgets.readthedocs.io/en/stable/user_install.html\n",
      "  from .autonotebook import tqdm as notebook_tqdm\n"
     ]
    }
   ],
   "source": [
    "import sys\n",
    "import os\n",
    "import numpy as np\n",
    "import pandas as pd\n",
    "import dill\n",
    "import tqdm\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "#sys.path.append(\"../../\")\n",
    "from trajectron.environment import Environment, Scene, Node\n",
    "from trajectron.environment import derivative_of\n",
    "\n",
    "from trap.tracker import Smoother"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "smoothing = True\n",
    "desired_max_time = 100\n",
    "pred_indices = [2, 3]\n",
    "state_dim = 6\n",
    "frame_diff = 10\n",
    "desired_frame_diff = 1\n",
    "dt = 0.1\n",
    "min_track_length = 20\n",
    "\n",
    "standardization = {\n",
    "    'PEDESTRIAN': {\n",
    "        'position': {\n",
    "            'x': {'mean': 0, 'std': 1},\n",
    "            'y': {'mean': 0, 'std': 1}\n",
    "        },\n",
    "        'velocity': {\n",
    "            'x': {'mean': 0, 'std': 2},\n",
    "            'y': {'mean': 0, 'std': 2}\n",
    "        },\n",
    "        'acceleration': {\n",
    "            'x': {'mean': 0, 'std': 1},\n",
    "            'y': {'mean': 0, 'std': 1}\n",
    "        }\n",
    "    }\n",
    "}\n",
    "\n",
    "data_columns = pd.MultiIndex.from_product([['position', 'velocity', 'acceleration'], ['x', 'y']])\n",
    "\n",
    "\n",
    "# desired_source = 'EXPERIMENTS/raw/hof-meter-maskrcnn2'\n",
    "# desired_source = 'EXPERIMENTS/20240424-hof-meter-maskrcnn2'\n",
    "# desired_source = 'EXPERIMENTS/20240426-hof-yolo'\n",
    "desired_source = 'EXPERIMENTS/raw/hof3'\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# run some tests\n",
    "\n",
    "if not os.path.exists(desired_source):\n",
    "    raise FileNotFoundError(f\"Path does not exist {desired_source=}\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "from matplotlib.axes import Axes\n",
    "from trap.frame_emitter import DetectionState\n",
    "\n",
    "if smoothing:\n",
    "    smoother = Smoother(window_len=12, convolution=False)\n",
    "\n",
    "def parse_txt(data_path, dt, axes: Axes, axes2: Axes) -> dict[str, pd.DataFrame]:\n",
    "    skipped_for_error = 0\n",
    "    created = 0\n",
    "\n",
    "    data = pd.read_csv(data_path, sep='\\t', index_col=False, header=None)\n",
    "    data.columns = ['frame_id', 'track_id', 'l','t', 'w','h', 'pos_x', 'pos_y', 'state']\n",
    "    # data['frame_id'] = pd.to_numeric(data['frame_id'], downcast='integer')\n",
    "    data['track_id'] = pd.to_numeric(data['track_id'], downcast='integer')\n",
    "\n",
    "    data['frame_id'] = data['frame_id'] // 10\n",
    "    data['frame_id'] -= data['frame_id'].min()\n",
    "\n",
    "    data['node_type'] = 'PEDESTRIAN'\n",
    "    data['node_id'] = data['track_id'].astype(str)\n",
    "    data['state'] = data['state'].apply(lambda x:  (eval(x) if type(x) is str else  DetectionState(x)).value)\n",
    "    data.sort_values('frame_id', inplace=True)\n",
    "\n",
    "    # Mean Position\n",
    "\n",
    "    print(\"Means: x:\", data['pos_x'].mean(), \"y:\", data['pos_y'].mean())\n",
    "    data['pos_x'] = data['pos_x'] - data['pos_x'].mean()\n",
    "    data['pos_y'] = data['pos_y'] - data['pos_y'].mean()\n",
    "\n",
    "    max_timesteps = data['frame_id'].max()\n",
    "\n",
    "    nodes = {}\n",
    "\n",
    "    # only keep tentative and confirmed detections\n",
    "    # print(data)\n",
    "    data = data.loc[data['state'] != DetectionState.Lost.value]\n",
    "\n",
    "    # print(data[['track_id', 'node_id', 'state']])\n",
    "\n",
    "    for node_id in tqdm.tqdm(pd.unique(data['node_id'])):\n",
    "        node_df = data[data['node_id'] == node_id]\n",
    "        if not np.all(np.diff(node_df['frame_id']) == 1):\n",
    "            # print(f\"Interval in {node_id} not always 1\")\n",
    "            # print(node_df['frame_id'])\n",
    "            # print(np.diff(node_df['frame_id']) != 1)\n",
    "            # mask=np.append(False, np.diff(node_df['frame_id']) != 1)\n",
    "            # print(node_df[mask]['frame_id'])\n",
    "            skipped_for_error += 1\n",
    "            continue\n",
    "\n",
    "\n",
    "        node_values = node_df[['pos_x', 'pos_y']].values\n",
    "\n",
    "        if node_values.shape[0] <= min_track_length:\n",
    "            continue\n",
    "\n",
    "        new_first_idx = node_df['frame_id'].iloc[0]\n",
    "\n",
    "        x = smoother.smooth(node_values[:, 0])\n",
    "        y = smoother.smooth(-node_values[:, 1]) #  OpenCV a right-handed one (y looking down) coordinate system\n",
    "        vx = derivative_of(x, dt)\n",
    "        vy = derivative_of(y, dt)\n",
    "        ax = derivative_of(vx, dt)\n",
    "        ay = derivative_of(vy, dt)\n",
    "\n",
    "        \n",
    "        axes.plot(x, y, alpha=.3)\n",
    "        axes.scatter(x, y, marker='x', alpha=.3)\n",
    "\n",
    "        nv = node_df[['l','t', 'w', 'h']].values\n",
    "        x = smoother.smooth(nv[:, 0] + .5*nv[:, 2])\n",
    "        y = smoother.smooth(nv[:, 1] + nv[:, 3])\n",
    "        axes2.plot(x, y, alpha=.3)\n",
    "        axes2.scatter(x, y, marker='x', alpha=.3)\n",
    "\n",
    "        nodes[node_id] = [x,y]\n",
    "        # data_dict = {'node_id': node_id, ('position', 'x'): x,\n",
    "        #                 ('position', 'y'): y,\n",
    "        #                 ('velocity', 'x'): vx,\n",
    "        #                 ('velocity', 'y'): vy,\n",
    "        #                 ('acceleration', 'x'): ax,\n",
    "        #                 ('acceleration', 'y'): ay}\n",
    "        \n",
    "        # node_data = pd.DataFrame(data_dict, columns=data_columns)\n",
    "        # nodes[node_id] = node_data\n",
    "    return nodes"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "tracked.txt\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Means: x: 11.879941019858627 y: 7.810144445001718\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 3232/3232 [00:58<00:00, 55.53it/s]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "tracked.txt\n",
      "Means: x: 12.897854652926158 y: 8.370203861117504\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 519/519 [00:02<00:00, 242.70it/s]\n"
     ]
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "tracked.txt\n",
      "Means: x: 12.625057777854769 y: 8.071677215100557\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 291/291 [00:01<00:00, 257.23it/s]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "345"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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
      "text/plain": [
       "<Figure size 2000x1600 with 2 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "fig = plt.figure(figsize=(20,16))\n",
    "ax1, ax2 = fig.subplots(2)\n",
    "\n",
    "import cv2\n",
    "# ax = fig1.gca()\n",
    "# ax2 = fig2.gca()\n",
    "im = cv2.imread(\"../DATASETS/hof2/output.png\")\n",
    "ax2.imshow(im)\n",
    "ax1.set_aspect(1)\n",
    "ax2.set_aspect(1)\n",
    "nodes = []\n",
    "for data_class in ['train', 'val', 'test']:\n",
    "    target_dir = os.path.join(desired_source, data_class)\n",
    "    for subdir, dirs, files in os.walk(target_dir):\n",
    "        for file in files:\n",
    "            if file.endswith('.txt'):\n",
    "                print(data_class, file)\n",
    "                input_data_dict = dict()\n",
    "                full_data_path = os.path.join(subdir, file)\n",
    "                nodes.extend(parse_txt(full_data_path, dt, ax1, ax2))\n",
    "                # break\n",
    "        # break\n",
    "    # break\n",
    "fig.show()\n",
    "len(nodes)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": ".venv",
   "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.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}