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/hof2'\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": 4,
   "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])\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": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Means: x: 1248.8047602075271 y: 808.9387779274865\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 2292/2292 [00:32<00:00, 71.09it/s]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "183"
      ]
     },
     "execution_count": 5,
     "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",
    "                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
}