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": 20,
   "metadata": {},
   "outputs": [],
   "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": 21,
   "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 = 10\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": 22,
   "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": 23,
   "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": 24,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Means: x: 1294.2634385663268 y: 820.3295331686878\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "100%|██████████| 1823/1823 [00:17<00:00, 105.14it/s]\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "191"
      ]
     },
     "execution_count": 24,
     "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
}