test_interaction.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "194dfcfb",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "_StoreAction(option_strings=['--weight'], dest='weight', nargs=None, const=None, default='/media/drl/datas/zyk/LaneGCN/results/lanegcn_interaction/36.000.ckpt', type=<class 'str'>, choices=None, help='checkpoint path', metavar='WEIGHT')"
      ]
     },
     "execution_count": 1,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import argparse\n",
    "import os\n",
    "os.umask(0)\n",
    "os.environ[\"MKL_NUM_THREADS\"] = \"8\"\n",
    "os.environ[\"NUMEXPR_NUM_THREADS\"] = \"8\"\n",
    "os.environ[\"OMP_NUM_THREADS\"] = \"8\"\n",
    "\n",
    "os.environ[\"CUDA_VISIBLE_DEVICES\"] = \"0，1，2，3\"\n",
    "\n",
    "import pickle\n",
    "import sys\n",
    "from importlib import import_module\n",
    "\n",
    "import torch\n",
    "from torch.utils.data import DataLoader, Sampler\n",
    "from torch.utils.data.distributed import DistributedSampler\n",
    "from tqdm import tqdm\n",
    "\n",
    "from data import InteDataset\n",
    "from data import ArgoTestDataset\n",
    "from utils import Logger, load_pretrain\n",
    "from shapely.geometry import Polygon\n",
    "\n",
    "import numpy as np\n",
    "\n",
    "# define parser\n",
    "parser = argparse.ArgumentParser(description=\"Argoverse Motion Forecasting in Pytorch\")\n",
    "parser.add_argument(\n",
    "    \"-m\", \"--model\", default=\"lanegcn\", type=str, metavar=\"MODEL\", help=\"model name\"\n",
    ")\n",
    "parser.add_argument(\"--eval\", action=\"store_true\", default=True)\n",
    "parser.add_argument(\n",
    "    \"--split\", type=str, default=\"val\", help='data split, \"val\" or \"test\"'\n",
    ")\n",
    "parser.add_argument(\n",
    "    \"--weight\", default=\"/media/drl/datas/zyk/LaneGCN/results/lanegcn_interaction/36.000.ckpt\", type=str, metavar=\"WEIGHT\", help=\"checkpoint path\"\n",
    ")\n",
    "# parser.add_argument(\n",
    "#     \"--weight\", default=\"/media/drl/datas/zyk/LaneGCN/results/lanegcn/17.000.ckpt\", type=str, metavar=\"WEIGHT\", help=\"checkpoint path\"\n",
    "# )\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "329f4e90",
   "metadata": {},
   "outputs": [],
   "source": [
    "root_path = '/home/user/Backup/DRL服务器备份/Projects/LaneGCN_modified'\n",
    "sys.path.insert(0, root_path)\n",
    "\n",
    "model = import_module(\"lanegcn\")\n",
    "config, _, collate_fn, net, loss, post_process, opt = model.get_model()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "bcb496b5",
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/plain": [
       "{'display_iters': 79872,\n",
       " 'val_iters': 79872,\n",
       " 'save_freq': 0.1,\n",
       " 'epoch': 0,\n",
       " 'horovod': True,\n",
       " 'opt': 'adam',\n",
       " 'num_epochs': 80,\n",
       " 'lr': [0.001, 0.0001],\n",
       " 'lr_epochs': [32],\n",
       " 'lr_func': <utils.StepLR at 0x7fd9306bbe90>,\n",
       " 'save_dir': '/home/user/Backup/DRL服务器备份/Projects/LaneGCN_modified/results/lanegcn',\n",
       " 'batch_size': 16,\n",
       " 'val_batch_size': 16,\n",
       " 'workers': 0,\n",
       " 'val_workers': 0,\n",
       " 'train_split': '/home/user/Backup/DRL服务器备份/Projects/LaneGCN_modified/dataset/train/data',\n",
       " 'val_split': '/home/user/Backup/DRL服务器备份/Projects/LaneGCN_modified/dataset/val/data',\n",
       " 'test_split': '/home/user/Backup/DRL服务器备份/Projects/LaneGCN_modified/dataset/test_obs/data',\n",
       " 'preprocess': True,\n",
       " 'preprocess_train': '/home/user/Backup/DRL服务器备份/Projects/LaneGCN_modified/dataset/preprocess/val_crs_dist6_angle90.p',\n",
       " 'preprocess_val': '/home/user/Backup/DRL服务器备份/Projects/LaneGCN_modified/dataset/preprocess/val_crs_dist6_angle90.p',\n",
       " 'preprocess_test': '/home/user/Backup/DRL服务器备份/Projects/LaneGCN_modified/dataset/preprocess/test_test.p',\n",
       " 'rot_aug': False,\n",
       " 'pred_range': [-100.0, 100.0, -100.0, 100.0],\n",
       " 'num_scales': 7,\n",
       " 'n_actor': 128,\n",
       " 'n_map': 128,\n",
       " 'actor2map_dist': 7.0,\n",
       " 'map2actor_dist': 6.0,\n",
       " 'actor2actor_dist': 100.0,\n",
       " 'pred_size': 80,\n",
       " 'pred_step': 1,\n",
       " 'num_preds': 80,\n",
       " 'num_mods': 6,\n",
       " 'cls_coef': 1.0,\n",
       " 'reg_coef': 1.0,\n",
       " 'mgn': 0.2,\n",
       " 'cls_th': 2.0,\n",
       " 'cls_ignore': 0.2}"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "config"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "3fdf2e44",
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "## 10s multi LL\n",
    "ckpt_path = \"/home/user/Backup/DRL服务器备份/Projects/interaction_gyt/preprocess_results_10s_for_training_EEMG/results_multi_ll/80.000.ckpt\"\n",
    "\n",
    "## 10s single LL\n",
    "# ckpt_path = \"/home/user/Projects/interaction_gyt/preprocess_results_10s_for_training_EEMG/results_0812/41.000.ckpt\"\n",
    "\n",
    "## 10s GCN\n",
    "# ckpt_path = \"/home/user/Datasets/interpolated/preprocess_results_10s_interp10scale8/results/47.000.ckpt\"\n",
    "\n",
    "## 5s multi LL\n",
    "# ckpt_path = \"/home/user/Projects/interaction_gyt/preprocess_results_5s_for_training_EEGM/results_multi_ll/17.000.ckpt\"\n",
    "\n",
    "## 5s single LL\n",
    "# ckpt_path = \"/home/user/Projects/interaction_gyt/preprocess_results_5s_for_training_EEGM/results_single_ll/12.000.ckpt\"\n",
    "\n",
    "## 5s GCN\n",
    "# ckpt_path = \"/home/user/Projects/LaneGCN_modified/results/history_results/raw_map_results/lanegcn_5s/36.000.ckpt\"\n",
    "\n",
    "\n",
    "if not os.path.isabs(ckpt_path):\n",
    "    ckpt_path = os.path.join(config[\"save_dir\"], ckpt_path)\n",
    "ckpt = torch.load(ckpt_path, map_location=lambda storage, loc: storage)\n",
    "load_pretrain(net, ckpt[\"state_dict\"])\n",
    "net.eval()\n",
    "\n",
    "\n",
    "with open(os.path.join(\"/home/user/Backup/DRL服务器备份/Logs/0914\", 'EP0_all_940_1070_950_1040_mask.pkl'), 'rb') as f:\n",
    "    img_EP0 = pickle.load(f, encoding=\"latin1\")\n",
    "    EP0_bds = (940, 1070, 950, 1040)\n",
    "with open(os.path.join(\"/home/user/Backup/DRL服务器备份/Logs/0914\", 'EP1_all_942_1115_949_1032_mask.pkl'), 'rb') as f:\n",
    "    img_EP1 = pickle.load(f, encoding=\"latin1\")\n",
    "    EP1_bds = (942, 1115, 949, 1032)\n",
    "with open(os.path.join(\"/home/user/Backup/DRL服务器备份/Logs/0914\", 'GL_all_920_1040_940_1025_mask.pkl'), 'rb') as f:\n",
    "    img_GL = pickle.load(f, encoding=\"latin1\")\n",
    "    GL_bds = (920, 1040, 940, 1025)\n",
    "with open(os.path.join(\"/home/user/Backup/DRL服务器备份/Logs/0914\", 'MA_all_970_1080_960_1050_mask.pkl'), 'rb') as f:\n",
    "    img_MA = pickle.load(f, encoding=\"latin1\")\n",
    "    MA_bds = (970, 1080, 960, 1050)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "6fc92f91",
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'InteDataset' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m/tmp/ipykernel_207222/871039198.py\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m     25\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     26\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 27\u001b[0;31m \u001b[0mdataset\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mInteDataset\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mfilepath\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     28\u001b[0m \u001b[0;31m# dataset = InteDataset('/home/user/Datasets/raw_map/preprocess_results_5s/tmp/')\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     29\u001b[0m data_loader = DataLoader(\n",
      "\u001b[0;31mNameError\u001b[0m: name 'InteDataset' is not defined"
     ]
    }
   ],
   "source": [
    "# 生成RL所需轨迹\n",
    "\n",
    "## 10s test\n",
    "# filepath = '/home/user/Projects/interaction_gyt/preprocess_results_10s_for_testing_EP0/test/'\n",
    "\n",
    "## 10s csv\n",
    "# filepath = '/home/user/Backup/DRL服务器备份/Projects/interaction_gyt/preprocess_results_10s_use_csv_EP0_with_agtid_final/test'\n",
    "filepath = '/home/user/Backup/DRL服务器备份/Logs/1801/FT_SCR_py3/'\n",
    "\n",
    "## 5s test\n",
    "# filepath = '/home/user/Projects/interaction_gyt/preprocess_results_5s_for_testing_EP0/test/'\n",
    "\n",
    "## 5s csv\n",
    "# filepath = '/home/user/Projects/interaction_gyt/preprocess_results_5s_use_csv_EP0/test/'\n",
    "\n",
    "# 10s fake\n",
    "# filepath = '/home/user/Projects/interaction_gyt/preprocess_results_10s_for_testing_fake_EP0/test'\n",
    "\n",
    "# 10 EEMG\n",
    "# filepath = '/home/user/Projects/interaction_gyt/preprocess_results_10s_for_testing_EEMG/test'\n",
    "\n",
    "# 5 EEMG\n",
    "# filepath = '/home/user/Projects/interaction_gyt/preprocess_results_5s_for_training_EEGM/test'\n",
    "\n",
    "\n",
    "\n",
    "dataset = InteDataset(filepath)\n",
    "# dataset = InteDataset('/home/user/Datasets/raw_map/preprocess_results_5s/tmp/')\n",
    "data_loader = DataLoader(\n",
    "    dataset,\n",
    "    batch_size=config[\"val_batch_size\"],\n",
    "    num_workers=config[\"val_workers\"],\n",
    "    collate_fn=collate_fn,\n",
    "    shuffle=False,\n",
    "    pin_memory=False,\n",
    ")\n",
    "\n",
    "# begin inference\n",
    "preds = {}\n",
    "gts = {}\n",
    "cities = {}\n",
    "\n",
    "data_list = []\n",
    "off_road_count = 0\n",
    "total_count = 0\n",
    "k = 0\n",
    "\n",
    "first_total_count = 0\n",
    "first_off_road_count = 0\n",
    "\n",
    "collision_trajs_count = 0\n",
    "first_collision_trajs_count = 0\n",
    "\n",
    "map_collision = {\n",
    "    \"P0\": [0, 0, 0, 0],\n",
    "    \"P1\": [0, 0, 0, 0],\n",
    "    \"GL\": [0, 0, 0, 0],\n",
    "    \"MA\": [0, 0, 0, 0]\n",
    "}\n",
    "\n",
    "max_acc_rcd, min_acc_rcd = [], []\n",
    "all_beta_rcd, max_beta_rcd = [], []\n",
    "\n",
    "cC, cR, cA = 0,0,0\n",
    "cRC, cRA, cCA = 0,0,0\n",
    "cRCA = 0\n",
    "\n",
    "for ii, data in tqdm(enumerate(data_loader)):\n",
    "    data = dict(data)\n",
    "    with torch.no_grad():\n",
    "        \n",
    "        output = net(data)\n",
    "        loss_out = loss(output, data)\n",
    "        results = [x.detach().cpu().numpy() for x in output[\"reg\"]]\n",
    "\n",
    "        for idx in range(len(results)):\n",
    "            filename = data['filename'][idx].split('/')[-1]\n",
    "            with open(os.path.join(filepath, filename), 'rb') as f:\n",
    "                origin_data = pickle.load(f, encoding=\"latin1\")\n",
    "\n",
    "            if filename[-6:-4] == \"P0\":\n",
    "                use_img = img_EP0\n",
    "                min_x, max_x, min_y, max_y = EP0_bds\n",
    "            elif filename[-6:-4] == \"P1\":\n",
    "                use_img = img_EP1    \n",
    "                min_x, max_x, min_y, max_y = EP1_bds\n",
    "            elif filename[-6:-4] == \"GL\":\n",
    "                use_img = img_GL\n",
    "                min_x, max_x, min_y, max_y = GL_bds\n",
    "            else:\n",
    "                use_img = img_MA\n",
    "                min_x, max_x, min_y, max_y = MA_bds\n",
    "            \n",
    "            for k in range(1):\n",
    "                new_data = {}\n",
    "#                 ego_id = origin_data['idx']\n",
    "                new_data['egos_track'] = {}\n",
    "                for a in range(origin_data['shapes'].shape[0]):\n",
    "                    ego_id = origin_data['agt_ids'][a]\n",
    "                    traj_xyphi, traj_interval_vel = interval(data, results, idx, a, k)\n",
    "                    if traj_interval_vel.shape[0] < 2:\n",
    "                        print(\"too short! idx \", idx, 'traj ', traj_interval_vel)\n",
    "                        continue\n",
    "                    new_data['egos_track'][ego_id] = [[filename.split('_')[1], filename.split('_')[2]]] + traj_interval_vel[:, [0,1,3]].tolist()\n",
    "\n",
    "\n",
    "                new_data['others_track'] = {}\n",
    "                new_data['gt_of_acc_violation'] = {}\n",
    "                new_data['gt_of_rotation'] = {}\n",
    "                start_time = int(filename.split('_')[1])\n",
    "                end_time = int(filename.split('_')[2])\n",
    "                \n",
    "#                 if origin_data['shapes'].shape[0] != 2:\n",
    "#                     print(\"not 2! \", data['filename'][idx], origin_data['shapes'].shape[0])\n",
    "                for a in range(origin_data['shapes'].shape[0]):\n",
    "                    other_id = origin_data['agt_ids'][a]\n",
    "                    new_data['others_track'][other_id] = [origin_data['shapes'][a].tolist() + [start_time, end_time]]\n",
    "\n",
    "\n",
    "                    traj_xyphi, traj_interval_vel = interval(data, results, idx, a, k)\n",
    "                    origin_yaw = traj_interval_vel[0, 2] % (2 * np.pi)\n",
    "\n",
    "#                     if np.pi / 4 <= origin_yaw and origin_yaw < np.pi * 3 / 4:\n",
    "#                         use_img = img_from_bot\n",
    "#                         tag = \"bot\"\n",
    "#                     elif np.pi * 3 / 4 <= origin_yaw and origin_yaw < np.pi * 5 / 4:\n",
    "#                         use_img = img_from_right\n",
    "#                         tag = \"right\"\n",
    "#                     elif np.pi * 5 / 4 <= origin_yaw and origin_yaw < np.pi * 7 / 4:\n",
    "#                         use_img = img_from_up\n",
    "#                         tag = \"up\"\n",
    "#                     else:\n",
    "#                         use_img = img_from_left \n",
    "#                         tag = \"left\"\n",
    "\n",
    "\n",
    "                    flag_off_road = False\n",
    "#                     use_img = img_all\n",
    "                    \n",
    "                    use_ground_truth = False\n",
    "                    max_acc = float('-inf')\n",
    "                    min_acc = float('inf')\n",
    "                    max_beta = 0\n",
    "                \n",
    "                    tmp_traj = np.concatenate([data['gt_hist'][idx][a][:, :2], data['gt_preds'][idx][a]], axis=0)\n",
    "#                     tmp_traj = np.concatenate([data['gt_hist'][idx][a][:, :2], results[idx][a][0]], axis=0)\n",
    "                    for ttt in range(traj_xyphi.shape[0]):\n",
    "                        if ttt > 1:\n",
    "                            if use_ground_truth:\n",
    "                                dx0, dy0 = tmp_traj[ttt][0] - tmp_traj[ttt-1][0], tmp_traj[ttt][1] - tmp_traj[ttt-1][1]\n",
    "                                cur_v = np.sqrt(dx0 ** 2 + dy0 ** 2) * 10\n",
    "                                cur_yaw = math.atan2(dy0, dx0)\n",
    "                                \n",
    "                                dx1, dy1 = tmp_traj[ttt-1][0] - tmp_traj[ttt-2][0], tmp_traj[ttt-1][1] - tmp_traj[ttt-2][1]\n",
    "                                last_v = np.sqrt(dx1 ** 2 + dy1 ** 2) * 10\n",
    "                                last_yaw = math.atan2(dy1, dx1)\n",
    "                            else:\n",
    "                                cur_v = np.sqrt(traj_xyphi[ttt][3] ** 2 + traj_xyphi[ttt][4] ** 2)\n",
    "                                last_v = np.sqrt(traj_xyphi[ttt-1][3] ** 2 + traj_xyphi[ttt-1][4] ** 2)                            \n",
    "                                cur_yaw = traj_xyphi[ttt][2]\n",
    "                                last_yaw = traj_xyphi[ttt-1][2]\n",
    "                                \n",
    "                            beta = min((cur_yaw - last_yaw) % 6.28, (last_yaw - cur_yaw) % 6.28)\n",
    "                            all_beta_rcd.append(beta)\n",
    "                            max_beta = max(max_beta, beta)\n",
    "                            \n",
    "                            acc = (cur_v - last_v) * 10\n",
    "                            max_acc = max(max_acc, acc)\n",
    "                            min_acc = min(min_acc, acc)\n",
    "                        \n",
    "                        timestamp = start_time + ttt * 100\n",
    "                        x, y, phi, vx, vy = traj_xyphi[ttt]\n",
    "                        addinfo = [timestamp, x, y, vx, vy, phi]\n",
    "                        new_data['others_track'][other_id].append(addinfo)\n",
    "\n",
    "\n",
    "#                         if x < min_x or x > max_x or y < min_y or y > max_y:\n",
    "#                         # 正常行驶至道路末端\n",
    "# #                         if x < 943 or x > 1065 or y < 958 or y > 1028: \n",
    "#                             continue\n",
    "            \n",
    "#                         grid_x, grid_y = int((x - min_x) / (max_x - min_x + 1) * use_img.shape[1]), int((max_y - y) / (max_y - min_y + 1) * use_img.shape[0])\n",
    "# #                         grid_x, grid_y = int((max_x - min_x) * 10 * (x - min_x) / (max_x - min_x + 1)), int((max_y - min_y) * 10 * (max_y - y) / (max_y - min_y + 1))\n",
    "#                         if use_img[grid_y, grid_x] != 0:\n",
    "# #                             print(\"tag from %s agt %d fail at time %d, pos %f, %f, value %d\" %(tag, a, ttt, x, y, use_img[grid_y, grid_x]))\n",
    "#                             flag_off_road = True\n",
    "                    \n",
    "                    if True:\n",
    "                        other_id = origin_data['agt_ids'][a]\n",
    "                        new_data['gt_of_acc_violation'][other_id] = np.concatenate([data['gt_hist'][idx][a][:, :2], data['gt_preds'][idx][a]], axis=0)             \n",
    "                \n",
    "                    if max_beta > 3.0:\n",
    "                        other_id = origin_data['agt_ids'][a]\n",
    "                        new_data['gt_of_rotation'][other_id] = np.concatenate([data['gt_hist'][idx][a][:, :2], data['gt_preds'][idx][a]], axis=0)             \n",
    "                \n",
    "                    max_beta_rcd.append(max_beta)\n",
    "                    max_acc_rcd.append(max_acc)\n",
    "                    min_acc_rcd.append(min_acc)\n",
    "                                        \n",
    "                    total_count += 1\n",
    "                    map_collision[filename[-6:-4]][1] += 1\n",
    "                    if flag_off_road:\n",
    "                        off_road_count += 1\n",
    "                        map_collision[filename[-6:-4]][0] += 1\n",
    "                        \n",
    "                    if k == 0:\n",
    "                        map_collision[filename[-6:-4]][3] += 1\n",
    "                        first_total_count += 1\n",
    "                        if flag_off_road:\n",
    "                            first_off_road_count += 1\n",
    "                            map_collision[filename[-6:-4]][2] += 1\n",
    "                            \n",
    "                            \n",
    "                N = len(new_data['others_track'].keys())\n",
    "                collision = np.zeros(N)\n",
    "                \n",
    "                for time_stamp in range(100):\n",
    "                    polygon_coll = []\n",
    "                    for a in new_data['others_track'].keys():\n",
    "                        tmp = np.zeros([3])\n",
    "                        tmp[0] = new_data['others_track'][a][1:][time_stamp][1]\n",
    "                        tmp[1] = new_data['others_track'][a][1:][time_stamp][2]\n",
    "                        tmp[2] = new_data['others_track'][a][1:][time_stamp][5]\n",
    "\n",
    "#                         if a not in new_data['egos_track'].keys():\n",
    "#                             tmp[0] = new_data['others_track'][a][1:][0][1]\n",
    "#                             tmp[1] = new_data['others_track'][a][1:][0][2]\n",
    "#                             tmp[2] = new_data['others_track'][a][1:][0][5]   \n",
    "                            \n",
    "                        width = new_data['others_track'][a][0][0]\n",
    "                        length = new_data['others_track'][a][0][1]\n",
    "                        ms = tmp\n",
    "                        polygon_coll.append(Polygon(polygon_xy_from_motionstate(ms, width, length)))\n",
    "\n",
    "                    for i in range(len(polygon_coll)):\n",
    "                        p1 = polygon_coll[i]\n",
    "                        for j in range(i+1, len(polygon_coll)):\n",
    "                            p2 = polygon_coll[j]\n",
    "                            \n",
    "                            if(p1.intersects(p2)):\n",
    "                                collision[i] = 1\n",
    "                                collision[j] = 1\n",
    "                collision_trajs_count += np.sum(collision)\n",
    "                if k == 0:\n",
    "                    first_collision_trajs_count += np.sum(collision)\n",
    "    \n",
    "                new_data['gt_of_collision'] = {}\n",
    "                for a in range(origin_data['shapes'].shape[0]):\n",
    "                    if collision[a] == 1:\n",
    "                        other_id = origin_data['agt_ids'][a]\n",
    "                        new_data['gt_of_collision'][other_id] = np.concatenate([data['gt_hist'][idx][a][:, :2], data['gt_preds'][idx][a]], axis=0)\n",
    "    \n",
    "                new_data['gt_of_trouble'] = {}\n",
    "                for key in new_data['gt_of_collision'].keys():\n",
    "                    new_data['gt_of_trouble'][key] = new_data['gt_of_collision'][key]\n",
    "                \n",
    "                for key in new_data['gt_of_acc_violation'].keys():\n",
    "                    new_data['gt_of_trouble'][key] = new_data['gt_of_acc_violation'][key]\n",
    "                \n",
    "                for key in new_data['gt_of_rotation'].keys():\n",
    "                    new_data['gt_of_trouble'][key] = new_data['gt_of_rotation'][key]\n",
    "                \n",
    "                for key in new_data['gt_of_trouble'].keys():\n",
    "                    if key in new_data['gt_of_collision']:\n",
    "                        cC += 1\n",
    "                    if key in new_data['gt_of_rotation']:\n",
    "                        cR += 1\n",
    "                    if key in new_data['gt_of_acc_violation']:\n",
    "                        cA += 1\n",
    "                        \n",
    "                    if key in new_data['gt_of_collision'] and key in new_data['gt_of_rotation']:\n",
    "                        cRC += 1\n",
    "                    if key in new_data['gt_of_acc_violation'] and key in new_data['gt_of_rotation']:\n",
    "                        cRA += 1\n",
    "                    if key in new_data['gt_of_collision'] and key in new_data['gt_of_acc_violation']:\n",
    "                        cCA += 1\n",
    "                    if key in new_data['gt_of_collision'] and key in new_data['gt_of_rotation'] and key in new_data['gt_of_acc_violation']:\n",
    "                        cRCA += 1\n",
    "                \n",
    "#             if ii == 1:\n",
    "#                 break\n",
    "\n",
    "#         if ii == 1:\n",
    "#             break\n",
    "\n",
    "#             with open(os.path.join('/home/user/Projects/interaction_gyt/preprocess_results_10s_use_csv_EP0_with_agtid_final/test_0913_3', filename), 'wb') as f22:\n",
    "#                 pickle.dump(new_data, f22, protocol=pickle.HIGHEST_PROTOCOL)\n",
    "            with open(os.path.join('home/user/Backup/DRL服务器备份/Logs/1801/test_FT_SCR_py3', filename), 'wb') as f22:\n",
    "                pickle.dump(new_data, f22, protocol=pickle.HIGHEST_PROTOCOL)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "680fd10f",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "828 56 19 14 0 4 0\n",
      "14 0 4 0\n"
     ]
    }
   ],
   "source": [
    "print(cC, cR, cA, cRC, cRA, cCA, cRCA)\n",
    "print(cRC, cRA, cCA, cRCA)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "id": "5c9c9275",
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "P0 total 0.062829, first 0.062829\n"
     ]
    },
    {
     "ename": "ZeroDivisionError",
     "evalue": "division by zero",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mZeroDivisionError\u001b[0m                         Traceback (most recent call last)",
      "\u001b[0;32m/tmp/ipykernel_28922/3290858030.py\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      1\u001b[0m \u001b[0;32mfor\u001b[0m \u001b[0mkey\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mmap_collision\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mkeys\u001b[0m\u001b[0;34m(\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      2\u001b[0m     \u001b[0mA\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mmap_collision\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mkey\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 3\u001b[0;31m     \u001b[0mprint\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mkey\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m\"total %f, first %f\"\u001b[0m \u001b[0;34m%\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mA\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m0\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m/\u001b[0m\u001b[0mA\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mA\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m2\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m/\u001b[0m\u001b[0mA\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m3\u001b[0m\u001b[0;34m]\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[0;31mZeroDivisionError\u001b[0m: division by zero"
     ]
    }
   ],
   "source": [
    "for key in map_collision.keys():\n",
    "    A = map_collision[key]\n",
    "    print(key, \"total %f, first %f\" %(A[0]/A[1], A[2]/A[3]))\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "id": "e361f657",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0.03846754554632553"
      ]
     },
     "execution_count": 27,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAXQAAAD4CAYAAAD8Zh1EAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjUuMCwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy8/fFQqAAAACXBIWXMAAAsTAAALEwEAmpwYAAANmklEQVR4nO3df6jd913H8eerSePE1hXMFUuS9VbMwDimrZesUtDCOkg7SP7YkAR0dtQF1OpkQ4g/6Gb9Z3MwYS5aoyv7gbbrqozrmhKHVgZia263tjaJGddYTWKhd13tHNXVyNs/7sk8vTn3nu9tzr3n3M+eD7hwvt/vh3Pe+bZ59tzv+dFUFZKkje+KcQ8gSRoNgy5JjTDoktQIgy5JjTDoktSIzeN64K1bt9b09PS4Hl6SNqQnnnjia1U1NejY2II+PT3N3NzcuB5ekjakJP+63DEvuUhSIwy6JDXCoEtSIwy6JDViaNCT3Jfk+STPLHM8ST6WZD7J00luHP2YkqRhujxD/ySwZ4XjtwE7ez8HgT+8/LEkSas1NOhV9SXg6yss2Qd8uhY9BlyT5NpRDShJ6mYU19C3AWf7ts/19l0iycEkc0nmFhYWRvDQkqSL1vVF0ao6UlUzVTUzNTXwg06SpNdoFJ8UPQ/s6Nve3tu3ZqYPPXzJvmc/9Pa1fEhJmnijeIY+C7yr926Xm4CXquq5EdyvJGkVhj5DT3I/cAuwNck54APAlQBVdS9wFLgdmAdeBt69VsNKkpY3NOhVdWDI8QJ+aWQTSZJeEz8pKkmNMOiS1AiDLkmNMOiS1AiDLkmNMOiS1AiDLkmNMOiS1AiDLkmNMOiS1AiDLkmNMOiS1AiDLkmNMOiS1AiDLkmNMOiS1AiDLkmNMOiS1AiDLkmNMOiS1AiDLkmNMOiS1AiDLkmNMOiS1AiDLkmNMOiS1AiDLkmNMOiS1AiDLkmNMOiS1AiDLkmNMOiS1IhOQU+yJ8npJPNJDg04/oYkjyb5SpKnk9w++lElSSsZGvQkm4DDwG3ALuBAkl1Llv0W8GBV3QDsB/5g1INKklbW5Rn6bmC+qs5U1SvAA8C+JWsK+N7e7dcD/z66ESVJXWzusGYbcLZv+xzwliVrPgj8VZJfBr4HuHUk00mSOhvVi6IHgE9W1XbgduAzSS657yQHk8wlmVtYWBjRQ0uSoFvQzwM7+ra39/b1uxN4EKCq/h54HbB16R1V1ZGqmqmqmampqdc2sSRpoC5BPw7sTHJ9ki0svug5u2TNvwFvBUjywywG3afgkrSOhga9qi4AdwHHgFMsvpvlRJJ7kuztLXs/8J4kTwH3A3dUVa3V0JKkS3V5UZSqOgocXbLv7r7bJ4GbRzuaJGk1/KSoJDXCoEtSIwy6JDXCoEtSIwy6JDXCoEtSIwy6JDXCoEtSIwy6JDXCoEtSIwy6JDXCoEtSIwy6JDXCoEtSIwy6JDXCoEtSIwy6JDXCoEtSIwy6JDXCoEtSIwy6JDXCoEtSIwy6JDXCoEtSIwy6JDXCoEtSIwy6JDXCoEtSIwy6JDXCoEtSIwy6JDXCoEtSIwy6JDWiU9CT7ElyOsl8kkPLrPnpJCeTnEjyZ6MdU5I0zOZhC5JsAg4DbwPOAceTzFbVyb41O4FfB26uqheTfP9aDSxJGqzLM/TdwHxVnamqV4AHgH1L1rwHOFxVLwJU1fOjHVOSNEyXoG8DzvZtn+vt6/dG4I1J/i7JY0n2DLqjJAeTzCWZW1hYeG0TS5IGGtWLopuBncAtwAHgj5Ncs3RRVR2pqpmqmpmamhrRQ0uSoFvQzwM7+ra39/b1OwfMVtX/VNW/AF9lMfCSpHXSJejHgZ1Jrk+yBdgPzC5Z83kWn52TZCuLl2DOjG5MSdIwQ4NeVReAu4BjwCngwao6keSeJHt7y44BLyQ5CTwK/FpVvbBWQ0uSLjX0bYsAVXUUOLpk3919twt4X+9HkjQGflJUkhph0CWpEQZdkhph0CWpEQZdkhph0CWpEQZdkhph0CWpEQZdkhph0CWpEQZdkhph0CWpEQZdkhph0CWpEQZdkhph0CWpEQZdkhph0CWpEQZdkhph0CWpEQZdkhph0CWpEQZdkhph0CWpEQZdkhph0CWpEQZdkhph0CWpEQZdkhph0CWpEQZdkhph0CWpEQZdkhrRKehJ9iQ5nWQ+yaEV1r0jSSWZGd2IkqQuhgY9ySbgMHAbsAs4kGTXgHVXA+8FHh/1kJKk4bo8Q98NzFfVmap6BXgA2Ddg3e8AHwb+e4TzSZI66hL0bcDZvu1zvX3fluRGYEdVPbzSHSU5mGQuydzCwsKqh5UkLe+yXxRNcgXwUeD9w9ZW1ZGqmqmqmampqct9aElSny5BPw/s6Nve3tt30dXAm4C/TfIscBMw6wujkrS+ugT9OLAzyfVJtgD7gdmLB6vqparaWlXTVTUNPAbsraq5NZlYkjTQ0KBX1QXgLuAYcAp4sKpOJLknyd61HlCS1M3mLouq6ihwdMm+u5dZe8vljyVJWi0/KSpJjTDoktQIgy5JjTDoktQIgy5JjTDoktQIgy5JjTDoktQIgy5JjTDoktQIgy5JjTDoktQIgy5JjTDoktQIgy5JjTDoktQIgy5JjTDoktQIgy5JjTDoktQIgy5JjTDoktQIgy5JjTDoktQIgy5JjTDoktQIgy5JjTDoktQIgy5JjTDoktQIgy5JjTDoktQIgy5JjegU9CR7kpxOMp/k0IDj70tyMsnTSf46yXWjH1WStJKhQU+yCTgM3AbsAg4k2bVk2VeAmap6M/AQ8LujHlSStLIuz9B3A/NVdaaqXgEeAPb1L6iqR6vq5d7mY8D20Y4pSRqmS9C3AWf7ts/19i3nTuCRQQeSHEwyl2RuYWGh+5SSpKFG+qJokp8BZoCPDDpeVUeqaqaqZqampkb50JL0HW9zhzXngR1929t7+14lya3AbwI/VVXfGs14kqSuujxDPw7sTHJ9ki3AfmC2f0GSG4A/AvZW1fOjH1OSNMzQoFfVBeAu4BhwCniwqk4kuSfJ3t6yjwBXAZ9L8mSS2WXuTpK0RrpccqGqjgJHl+y7u+/2rSOeS5K0Sn5SVJIaYdAlqREGXZIaYdAlqREGXZIaYdAlqREGXZIaYdAlqREGXZIaYdAlqREGXZIaYdAlqREGXZIaYdAlqREGXZIaYdAlqREGXZIaYdAlqREGXZIaYdAlqREGXZIaYdAlqREGXZIaYdAlqREGXZIaYdAlqREGXZIaYdAlqREGXZIaYdAlqREGXZIaYdAlqREGXZIa0SnoSfYkOZ1kPsmhAce/K8lne8cfTzI98kklSSsaGvQkm4DDwG3ALuBAkl1Llt0JvFhVPwT8HvDhUQ8qSVrZ5g5rdgPzVXUGIMkDwD7gZN+afcAHe7cfAj6eJFVVI5y1s+lDD1+y79kPvX0Mk0hq3ST1pkvQtwFn+7bPAW9Zbk1VXUjyEvB9wNf6FyU5CBzsbX4zyenXMPPWpfcLkCG/Eww7vo4Gzr9BbOTZwfnHbSPPv6rZ17g31y13oEvQR6aqjgBHLuc+ksxV1cyIRlp3G3n+jTw7OP+4beT5N8rsXV4UPQ/s6Nve3ts3cE2SzcDrgRdGMaAkqZsuQT8O7ExyfZItwH5gdsmaWeDnerffCfzNuK6fS9J3qqGXXHrXxO8CjgGbgPuq6kSSe4C5qpoFPgF8Jsk88HUWo79WLuuSzQTYyPNv5NnB+cdtI8+/IWaPT6QlqQ1+UlSSGmHQJakRExv0jfx1Ax1mvyPJQpInez8/P445l5PkviTPJ3lmmeNJ8rHen+/pJDeu94zL6TD7LUle6jv3d6/3jCtJsiPJo0lOJjmR5L0D1kzk+e84+8Se/ySvS/IPSZ7qzf/bA9ZMbHcAqKqJ+2Hxxdd/Bn4Q2AI8BexasuYXgXt7t/cDnx333KuY/Q7g4+OedYU/w08CNwLPLHP8duARIMBNwOPjnnkVs98CfGHcc64w/7XAjb3bVwNfHfDvz0Se/46zT+z5753Pq3q3rwQeB25asmYiu3PxZ1KfoX/76waq6hXg4tcN9NsHfKp3+yHgrUmyjjMup8vsE62qvsTiu5WWsw/4dC16DLgmybXrM93KOsw+0arquar6cu/2fwKnWPwkdr+JPP8dZ59YvfP5zd7mlb2fpe8amdTuAJN7yWXQ1w0s/RfjVV83AFz8uoFx6zI7wDt6vy4/lGTHgOOTrOufcVL9RO/X6keS/Mi4h1lO79f5G1h8pthv4s//CrPDBJ//JJuSPAk8D3yxqpY99xPWHWByg966vwSmq+rNwBf5///ia+19Gbiuqn4U+H3g8+MdZ7AkVwF/DvxqVX1j3POsxpDZJ/r8V9X/VtWPsfiJ+N1J3jTmkVZlUoO+kb9uYOjsVfVCVX2rt/knwI+v02yj0uWfz0Sqqm9c/LW6qo4CVybZOuaxXiXJlSwG8U+r6i8GLJnY8z9s9o1w/gGq6j+AR4E9Sw5NaneAyQ36Rv66gaGzL7neuZfFa40bySzwrt67LW4CXqqq58Y9VBdJfuDiNc8ku1n8OzAxfyF7s30COFVVH11m2USe/y6zT/L5TzKV5Jre7e8G3gb805Jlk9odYJ2/bbGrmryvG+is4+y/kmQvcIHF2e8Y28ADJLmfxXcjbE1yDvgAiy8QUVX3AkdZfKfFPPAy8O7xTHqpDrO/E/iFJBeA/wL2T9JfSOBm4GeBf+xdywX4DeANMPHnv8vsk3z+rwU+lcX/qc8VwINV9YWN0J2L/Oi/JDViUi+5SJJWyaBLUiMMuiQ1wqBLUiMMuiQ1wqBLUiMMuiQ14v8AS2mLCq4hMEIAAAAASUVORK5CYII=\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# prediction\n",
    "tmp_N = 40\n",
    "tmp = np.zeros(tmp_N + 1)\n",
    "itv = (max(all_beta_rcd) - min(all_beta_rcd)) / tmp_N\n",
    "\n",
    "bb = min(all_beta_rcd)\n",
    "for beta in all_beta_rcd :\n",
    "    tmp[int((beta - bb) // itv)] += 1\n",
    "plt.bar([(i + 0.5) * itv for i in range(tmp_N + 1)],  (tmp / len(all_beta_rcd)), width=2/tmp_N)\n",
    "\n",
    "mean_beta = sum(all_beta_rcd) / len(all_beta_rcd)\n",
    "np.sum((np.array(all_beta_rcd) - mean_beta) ** 2) / len(all_beta_rcd)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "id": "a982523a",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "1.53358923937388"
      ]
     },
     "execution_count": 39,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "\n",
    "tmp = np.zeros(tmp_N + 1)\n",
    "itv = (max(max_beta_rcd) - min(max_beta_rcd)) / tmp_N\n",
    "\n",
    "bb = min(max_beta_rcd)\n",
    "for beta in max_beta_rcd :\n",
    "    tmp[int((beta - bb) // itv)] += 1\n",
    "plt.bar([(i + 0.5) * itv for i in range(tmp_N + 1)], tmp / len(max_beta_rcd), width=2/tmp_N)\n",
    "\n",
    "mean_beta = sum(max_beta_rcd) / len(max_beta_rcd)\n",
    "np.sum((np.array(max_beta_rcd) - mean_beta) ** 2) / len(max_beta_rcd)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6e5682c1",
   "metadata": {},
   "outputs": [],
   "source": [
    "# ground truth\n",
    "tmp_N = 40\n",
    "tmp = np.zeros(tmp_N + 1)\n",
    "itv = (max(all_beta_rcd) - min(all_beta_rcd)) / tmp_N\n",
    "\n",
    "bb = min(all_beta_rcd)\n",
    "for beta in all_beta_rcd :\n",
    "    tmp[int((beta - bb) // itv)] += 1\n",
    "# plt.plot([i * itv for i in range(tmp_N + 1)], 100 * tmp / len(max_acc_rcd))\n",
    "plt.bar([(i + 0.5) * itv for i in range(tmp_N + 1)], 100 * tmp / len(all_beta_rcd), width=2/tmp_N)\n",
    "\n",
    "\n",
    "mean_beta = sum(all_beta_rcd) / len(all_beta_rcd)\n",
    "np.sum((np.array(all_beta_rcd) - mean_beta) ** 2) / len(all_beta_rcd)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b11d1936",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "tmp = np.zeros(tmp_N + 1)\n",
    "itv = (max(max_beta_rcd) - min(max_beta_rcd)) / tmp_N\n",
    "\n",
    "bb = min(max_beta_rcd)\n",
    "for beta in max_beta_rcd :\n",
    "    tmp[int((beta - bb) // itv)] += 1\n",
    "# plt.plot([i * itv for i in range(tmp_N + 1)], 100 * tmp / len(max_acc_rcd))\n",
    "plt.bar([(i + 0.5) * itv for i in range(tmp_N + 1)], 100 * (tmp / len(max_beta_rcd)), width=2/tmp_N)\n",
    "\n",
    "mean_beta = sum(max_beta_rcd) / len(max_beta_rcd)\n",
    "np.sum((np.array(max_beta_rcd) - mean_beta) ** 2) / len(max_beta_rcd)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4bc19ca0",
   "metadata": {},
   "outputs": [],
   "source": [
    "# result\n",
    "tmp_N = 40\n",
    "tmp = np.zeros(tmp_N + 1)\n",
    "itv = (max(all_beta_rcd) - min(all_beta_rcd)) / tmp_N\n",
    "\n",
    "bb = min(all_beta_rcd)\n",
    "for beta in all_beta_rcd :\n",
    "    tmp[int((beta - bb) // itv)] += 1\n",
    "# plt.plot([i * itv for i in range(tmp_N + 1)], 100 * tmp / len(max_acc_rcd))\n",
    "plt.bar([(i + 0.5) * itv for i in range(tmp_N + 1)], 100 * tmp / len(all_beta_rcd), width=2/tmp_N)\n",
    "\n",
    "\n",
    "mean_beta = sum(all_beta_rcd) / len(all_beta_rcd)\n",
    "np.sum((np.array(all_beta_rcd) - mean_beta) ** 2) / len(all_beta_rcd)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 67,
   "id": "df609366",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "545"
      ]
     },
     "execution_count": 67,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "\n",
    "tmp = np.zeros(tmp_N + 1)\n",
    "itv = (max(max_beta_rcd) - min(max_beta_rcd)) / tmp_N\n",
    "\n",
    "bb = min(max_beta_rcd)\n",
    "for beta in max_beta_rcd :\n",
    "    tmp[int((beta - bb) // itv)] += 1\n",
    "# plt.plot([i * itv for i in range(tmp_N + 1)], 100 * tmp / len(max_acc_rcd))\n",
    "plt.bar([(i + 0.5) * itv for i in range(tmp_N + 1)], 100 * (tmp / len(max_beta_rcd)), width=2/tmp_N)\n",
    "\n",
    "mean_beta = sum(max_beta_rcd) / len(max_beta_rcd)\n",
    "np.sum((np.array(max_beta_rcd) - mean_beta) ** 2) / len(max_beta_rcd)\n",
    "\n",
    "np.sum(np.array(max_beta_rcd) >= 3.0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7acf11bf",
   "metadata": {},
   "outputs": [],
   "source": [
    "min_x, max_x, min_y, max_y\n",
    "\n",
    "tmp_N = 40\n",
    "tmp = np.zeros(tmp_N + 1)\n",
    "itv = max(max_acc_rcd) / tmp_N\n",
    "\n",
    "for acc in max_acc_rcd :\n",
    "    tmp[int(acc // itv)] += 1\n",
    "# plt.plot([i * itv for i in range(tmp_N + 1)], 100 * tmp / len(max_acc_rcd))\n",
    "plt.bar([(i + 0.5) * itv for i in range(tmp_N + 1)], 100 * tmp / len(max_acc_rcd), width=4/tmp_N)\n",
    "# max_acc_rcd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dbcc02e2",
   "metadata": {},
   "outputs": [],
   "source": [
    "max(min_acc_rcd)\n",
    "tmp_N = 40\n",
    "tmp = np.zeros(tmp_N + 1)\n",
    "itv = abs(min(min_acc_rcd)) / tmp_N\n",
    "\n",
    "for acc in min_acc_rcd :\n",
    "    tmp[int(abs(acc) // itv)] += 1\n",
    "plt.bar([(i + 0.5) * -itv for i in range(tmp_N + 1)], 100 * tmp / len(min_acc_rcd), width=4/tmp_N)\n",
    "# min_acc_rcd"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "9f278a95",
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'max_acc_rcd' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m/tmp/ipykernel_5827/2213555067.py\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      2\u001b[0m \u001b[0mtmp_N\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;36m40\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      3\u001b[0m \u001b[0mtmp\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mnp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mzeros\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mtmp_N\u001b[0m \u001b[0;34m*\u001b[0m \u001b[0;36m2\u001b[0m \u001b[0;34m+\u001b[0m \u001b[0;36m2\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 4\u001b[0;31m \u001b[0mitv\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mmax\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mmax\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mmax_acc_rcd\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m-\u001b[0m\u001b[0mmin\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mmin_acc_rcd\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;34m/\u001b[0m \u001b[0mtmp_N\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0m\u001b[1;32m      5\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      6\u001b[0m \u001b[0;32mfor\u001b[0m \u001b[0macc\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mmin_acc_rcd\u001b[0m \u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mNameError\u001b[0m: name 'max_acc_rcd' is not defined"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 720x432 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "fig, ax = plt.subplots(1,1,figsize = [10,6])\n",
    "tmp_N = 40 \n",
    "tmp = np.zeros(tmp_N * 2 + 2)\n",
    "itv = max(max(max_acc_rcd), -min(min_acc_rcd)) / tmp_N\n",
    "\n",
    "for acc in min_acc_rcd :\n",
    "    tmp[tmp_N - int(abs(acc) // itv)] += 1\n",
    "    \n",
    "for acc in max_acc_rcd :\n",
    "    tmp[tmp_N + int(acc // itv)] += 1\n",
    "    \n",
    "font2 = {'family':'Times New Roman',\n",
    "'size' : 25,\n",
    "}\n",
    "plt.tick_params(labelsize=25)\n",
    "labels = ax.get_xticklabels() + ax.get_yticklabels()\n",
    "[label.set_fontname('Times New Roman') for label in labels]\n",
    "    \n",
    "plt.bar([(i - 40 + 0.5) * itv for i in range(tmp_N * 2 + 2)], 100 * tmp / len(max_acc_rcd), width=3.9/tmp_N)\n",
    "plt.xlabel(\"Max acceleration m/s^2\", font2)\n",
    "plt.ylabel(\"Probability %\", font2)\n",
    "plt.savefig('/home/user/Logs/1102/acc', dpi=400)\n",
    "# plt.title(\"acceleration distribution \", font2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15a82196",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "453c26f3",
   "metadata": {},
   "outputs": [],
   "source": [
    "new_data['others_track'] = {}\n",
    "start_time = int(filename.split('_')[1])\n",
    "end_time = int(filename.split('_')[2])\n",
    "for iii in range(origin_data['shapes'].shape[0]):\n",
    "    other_id = origin_data['agt_ids'][iii]\n",
    "    new_data['others_track'][other_id] = [origin_data['shapes'][iii].tolist() + [start_time, end_time]]\n",
    "\n",
    "    traj_xyphi, traj_interval_vel = interval(data, results, idx, iii, k)\n",
    "    for ttt in range(100):\n",
    "        timestamp = start_time + ttt * 100\n",
    "        x, y, phi, vx, vy = traj_xyphi[ttt]\n",
    "        addinfo = [timestamp, x, y, vx/10, vy/10, phi]\n",
    "        new_data['others_track'][other_id].append(addinfo)\n",
    "    print(iii, other_id, new_data['others_track'][other_id][1])\n",
    "\n",
    "traj_xyphi, traj_interval_vel = interval(data, results, idx, 0, k)\n",
    "traj_xyphi"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bcaa8d62",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(off_road_count)\n",
    "print(total_count)\n",
    "print(1.0 * off_road_count / total_count)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3b2e52b1",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(first_off_road_count)\n",
    "print(first_total_count)\n",
    "print(1.0 * first_off_road_count / first_total_count)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "0abf654a",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "2849\n",
      "2849\n",
      "828.0\n",
      "828.0\n",
      "0.29062829062829065\n",
      "0.29062829062829065\n"
     ]
    }
   ],
   "source": [
    "# filepath = '/home/user/Projects/interaction_gyt/preprocess_results_10s_use_csv_EP0_with_agtid_final/test'\n",
    "print(total_count)\n",
    "print(first_total_count)\n",
    "print(collision_trajs_count)\n",
    "print(first_collision_trajs_count)\n",
    "print(1.0 * collision_trajs_count / total_count)\n",
    "print(1.0 * first_collision_trajs_count / first_total_count)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 61,
   "id": "ef634da6",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "6.282906282906283\n",
      "6.282906282906283\n",
      "29.062829062829064\n",
      "29.062829062829064\n"
     ]
    }
   ],
   "source": [
    "print(100.0 * off_road_count / total_count)\n",
    "print(100.0 * first_off_road_count / first_total_count)\n",
    "print(100.0 * collision_trajs_count / total_count)\n",
    "print(100.0 * first_collision_trajs_count / first_total_count)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "17db0b22",
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'data' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m<ipython-input-13-a860bba7daab>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[0;32m----> 1\u001b[0;31m \u001b[0mfeats\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdata\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'feats'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0midx\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      2\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      3\u001b[0m \u001b[0mfake_ctrs\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mnp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0marray\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;36m45\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m8\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m1\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      4\u001b[0m \u001b[0mtmp_ctrs\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mtorch\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcat\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mdata\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'ctrs'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0midx\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtorch\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mtensor\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mfake_ctrs\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;36m2\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mdtype\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0mtorch\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mfloat\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mreshape\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m-\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m)\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      5\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mNameError\u001b[0m: name 'data' is not defined"
     ]
    }
   ],
   "source": [
    "feats = data['feats'][idx]\n",
    "\n",
    "fake_ctrs = np.array([45, 8, 1])\n",
    "tmp_ctrs = torch.cat([data['ctrs'][idx], torch.tensor(fake_ctrs[:2], dtype=torch.float).reshape(1, -1)])\n",
    "\n",
    "fake_theta = np.pi\n",
    "# data['theta'].append(fake_theta)\n",
    "\n",
    "fake_feat = [[50-(50-fake_ctrs[0])*i/20 , fake_ctrs[1] , 1] for i in range(20)]\n",
    "fake_feat = torch.tensor(fake_feat, dtype=torch.float).reshape(1, 20, -1)\n",
    "tmp_feats = torch.cat([data['feats'][idx], fake_feat])\n",
    "# data['feats'][idx]\n",
    "\n",
    "tmp_has_preds = torch.cat([data['has_preds'][idx], data['has_preds'][idx][-1].reshape(1, -1)])\n",
    "\n",
    "fake_rot_ctrs = torch.tensor(fake_ctrs[:2], dtype=torch.float)\n",
    "fake_gt_preds = torch.cat([fake_rot_ctrs.reshape(1, -1) for i in range(config['pred_size'])], 0)\n",
    "fake_gt_preds = torch.matmul(fake_gt_preds, data['rot'][0]) + data['orig'][idx]\n",
    "tmp_gt_preds = torch.cat([data['gt_preds'][idx], fake_gt_preds.reshape(1, -1, 2)])\n",
    "\n",
    "                           \n",
    "tmp_steps = torch.cat([data['steps'][idx], data['steps'][idx][-1].reshape(1, -1)])\n",
    "\n",
    "data['has_preds'][idx] = tmp_has_preds\n",
    "data['gt_preds'][idx] = tmp_gt_preds\n",
    "data['feats'][idx] = tmp_feats\n",
    "data['theta'].append(fake_theta)\n",
    "data['ctrs'][idx] = tmp_ctrs\n",
    "\n",
    "with torch.no_grad():\n",
    "    data = dict(data)\n",
    "\n",
    "    output = net(data)\n",
    "    results = [x.detach().cpu().numpy() for x in output[\"reg\"]]\n",
    "    display = post_process(output, data)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "id": "9e098de8",
   "metadata": {},
   "outputs": [],
   "source": [
    "def traj_plot(traj, rot, orig, ax=None, c=\"\", s=[]):\n",
    "\n",
    "    \n",
    "    if isinstance(traj, torch.Tensor):\n",
    "        traj = traj.numpy()\n",
    "    rot = rot.numpy()\n",
    "    orig = orig.numpy()\n",
    "    \n",
    "    centerline = np.array([traj[:, 0] - orig[0], traj[:, 1] - orig[1]]).T\n",
    "#     print(centerline)\n",
    "    centerline = np.matmul(rot, (centerline.reshape(-1, 2)).T).T\n",
    "\n",
    "    if ax is not None:\n",
    "        ax.plot(centerline[:, 0], centerline[:, 1], c, s)\n",
    "        \n",
    "    return centerline\n",
    "\n",
    "def dis(x0, x1, y0, y1):\n",
    "    return np.sqrt((x0-x1)**2 + (y0-y1)**2)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "45101205",
   "metadata": {},
   "outputs": [],
   "source": [
    "data.keys()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "27518b55",
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "pred_traj = np.insert(results[idx][a, k, :, :], 2, values=None, axis=1)\n",
    "gt_hist = np.insert(data['gt_hist'][idx][a].cpu().detach().numpy(), 2, values=None, axis=1)\n",
    "gt_hist.shape"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "91c7456c",
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'data' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m/tmp/ipykernel_4119058/3817510654.py\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m     83\u001b[0m     \u001b[0;32mreturn\u001b[0m \u001b[0mtraj_to_show\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtraj_interval_vel\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     84\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 85\u001b[0;31m \u001b[0mtraj_to_show\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtraj_interval_vel\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0minterval\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mdata\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mresults\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0midx\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m2\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m0\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     86\u001b[0m \u001b[0mplt\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mplot\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mnp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msqrt\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mtraj_to_show\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m3\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m**\u001b[0m\u001b[0;36m2\u001b[0m \u001b[0;34m+\u001b[0m \u001b[0mtraj_to_show\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m4\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m**\u001b[0m \u001b[0;36m2\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     87\u001b[0m \u001b[0mtraj_to_show\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mNameError\u001b[0m: name 'data' is not defined"
     ]
    }
   ],
   "source": [
    "def interval(data, results, idx, a, k):\n",
    "    pred_traj = np.insert(results[idx][a, k, :, :], 2, values=None, axis=1)\n",
    "    gt_hist = data['gt_hist'][idx][a].cpu().detach().numpy()\n",
    "    traj_origin = np.concatenate([gt_hist, pred_traj])\n",
    "\n",
    "    # 起点\n",
    "    traj_interval = [traj_origin[0]]\n",
    "    velo_interval = [dis(traj_origin[1][0], traj_origin[0][0], traj_origin[1][1], traj_origin[0][1]) * 10]\n",
    "    \n",
    "#     Tseq = [0]\n",
    "    for i in range(1, traj_origin.shape[0] - 1):\n",
    "        if dis(traj_origin[i][0], traj_interval[-1][0], traj_origin[i][1], traj_interval[-1][1]) >= thrd:\n",
    "            traj_origin[i][2] = np.arctan2(traj_origin[i][1] - traj_interval[-1][1], traj_origin[i][0] - traj_interval[-1][0])\n",
    "            traj_interval.append(traj_origin[i])\n",
    "            vel = dis(traj_origin[i + 1][0], traj_origin[i - 1][0], traj_origin[i + 1][1], traj_origin[i - 1][1]) * 5\n",
    "            velo_interval.append(vel)\n",
    "        else:\n",
    "            traj_origin[i][2] = traj_origin[i - 1][2]\n",
    "            \n",
    "                         \n",
    "    # 终点\n",
    "    traj_origin[-1][2] = np.arctan2(traj_origin[-1][1] - traj_interval[-1][1], traj_origin[-1][0] - traj_interval[-1][0])\n",
    "    traj_interval.append(traj_origin[-1])\n",
    "    vel = dis(traj_origin[-1][0], traj_origin[-2][0], traj_origin[-1][1], traj_origin[-2][1]) * 10\n",
    "    velo_interval.append(vel)\n",
    "#     Tseq[-1] = traj_origin.shape[0] - 1\n",
    "\n",
    "    traj_interval_vel = np.concatenate([np.array(traj_interval), np.expand_dims(np.array(velo_interval), axis=1)], axis=1)\n",
    "    \n",
    "    \n",
    "    \n",
    "    X, Y, Yaw, VX, VY = [], [], [], [], []\n",
    "    for t in range(20):\n",
    "        X.append(gt_hist[t][0])\n",
    "        Y.append(gt_hist[t][1])        \n",
    "        Yaw.append(gt_hist[t][2])\n",
    "        if t == 0:\n",
    "            vx = gt_hist[1][0] - gt_hist[0][0]\n",
    "            vy = gt_hist[1][1] - gt_hist[0][1]\n",
    "        else:\n",
    "            vx = gt_hist[t][0] - gt_hist[t-1][0]\n",
    "            vy = gt_hist[t][1] - gt_hist[t-1][1]\n",
    "        VX.append(vx * 10)\n",
    "        VY.append(vy * 10)\n",
    "    \n",
    "\n",
    "    Tseq = [0]\n",
    "    Tsamples = [] \n",
    "    if config['pred_size'] == 80:\n",
    "        Tsamples = [20,40,60,79] \n",
    "    elif config['pred_size'] == 30:\n",
    "        Tsamples = [7,14,21,29]\n",
    "        \n",
    "    tscale = config['pred_size'] // 10\n",
    "    for t in Tsamples:\n",
    "        if dis(pred_traj[t, 0], pred_traj[Tseq[-1], 0], pred_traj[t, 0], pred_traj[Tseq[-1], 1]) >= 0.1:\n",
    "            Tseq.append(t)\n",
    "            \n",
    "            \n",
    "    sp = spline5(Xseq=pred_traj[Tseq, 0], \n",
    "                 Yseq=pred_traj[Tseq, 1],\n",
    "                 Tseq=Tseq,\n",
    "#                  yaw =gt_hist[-1, 2],\n",
    "                 vx=VX[-1] * tscale,\n",
    "                 vy=VY[-1] * tscale)\n",
    "\n",
    "    lastx, lasty = gt_hist[19][0], gt_hist[19][1]\n",
    "    for t in range(config['pred_size']):\n",
    "        x, y, dx, dy = sp.cal(t/(config['pred_size'] - 1))\n",
    "        X.append(x)\n",
    "        Y.append(y)\n",
    "        VX.append(dx / tscale)\n",
    "        VY.append(dy / tscale)\n",
    "        lastx, lasty = x, y        \n",
    "        Yaw.append(math.atan2(VY[-1], VX[-1]))\n",
    "\n",
    "    traj_to_show = np.concatenate([np.array(X).reshape(-1, 1), \n",
    "                                   np.array(Y).reshape(-1, 1),\n",
    "                                   np.array(Yaw).reshape(-1, 1),\n",
    "                                   np.array(VX).reshape(-1, 1),\n",
    "                                   np.array(VY).reshape(-1, 1)], axis=1)\n",
    "    \n",
    "    return traj_to_show, traj_interval_vel\n",
    "        \n",
    "traj_to_show, traj_interval_vel = interval(data, results, idx, 2, 0)\n",
    "plt.plot(np.sqrt(traj_to_show[:, 3]**2 + traj_to_show[:, 4] ** 2))\n",
    "traj_to_show"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c959bf3f",
   "metadata": {},
   "outputs": [],
   "source": [
    "results"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "d84a61a5",
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'interval' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m/tmp/ipykernel_5827/1513404476.py\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[0;32m----> 1\u001b[0;31m \u001b[0mtraj_xyphi\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mtraj_interval_vel\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0minterval\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mdata\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mresults\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0midx\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m0\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mk\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      2\u001b[0m \u001b[0mpred_traj\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mnp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0minsert\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mresults\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0midx\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0ma\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mk\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m:\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m:\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;36m2\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mvalues\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;32mNone\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0maxis\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;36m1\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      3\u001b[0m \u001b[0mgt_hist\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdata\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'gt_hist'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0midx\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0ma\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mcpu\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mdetach\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mnumpy\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      4\u001b[0m \u001b[0mtraj_origin\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mnp\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mconcatenate\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0mgt_hist\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0mpred_traj\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      5\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mNameError\u001b[0m: name 'interval' is not defined"
     ]
    }
   ],
   "source": [
    "traj_xyphi, traj_interval_vel = interval(data, results, idx, 0, k)\n",
    "pred_traj = np.insert(results[idx][a, k, :, :], 2, values=None, axis=1)\n",
    "gt_hist = data['gt_hist'][idx][a].cpu().detach().numpy()\n",
    "traj_origin = np.concatenate([gt_hist, pred_traj])\n",
    "\n",
    "traj_interval = [traj_origin[0]]\n",
    "velo_interval = [dis(traj_origin[1][0], traj_origin[0][0], traj_origin[1][1], traj_origin[0][1]) * 10]\n",
    "\n",
    "Tseq = [0]\n",
    "for i in range(1, traj_origin.shape[0] - 1):\n",
    "    if dis(traj_origin[i][0], traj_interval[-1][0], traj_origin[i][1], traj_interval[-1][1]) >= thrd:\n",
    "        if i > 20:\n",
    "            traj_origin[i][2] = np.arctan2(traj_origin[i][1] - traj_interval[-1][1], traj_origin[i][0] - traj_interval[-1][0])\n",
    "        traj_interval.append(traj_origin[i])\n",
    "        vel = dis(traj_origin[i + 1][0], traj_origin[i - 1][0], traj_origin[i + 1][1], traj_origin[i - 1][1]) * 5\n",
    "        velo_interval.append(vel)\n",
    "        Tseq.append(i)\n",
    "    else:\n",
    "        traj_origin[i][2] = traj_origin[i - 1][2]\n",
    "\n",
    "# 终点\n",
    "traj_origin[-1][2] = np.arctan2(traj_origin[-1][1] - traj_interval[-1][1], traj_origin[-1][0] - traj_interval[-1][0])\n",
    "traj_interval[-1] = traj_origin[-1]\n",
    "vel = dis(traj_origin[-1][0], traj_origin[-2][0], traj_origin[-1][1], traj_origin[-2][1]) * 10\n",
    "velo_interval[-1] = vel\n",
    "Tseq[-1] = traj_origin.shape[0] - 1\n",
    "\n",
    "traj_interval_vel = np.concatenate([np.array(traj_interval), np.expand_dims(np.array(velo_interval), axis=1)], axis=1)\n",
    "sp = spline5(Xseq=traj_interval_vel[:, 0], \n",
    "             Yseq=traj_interval_vel[:, 1],\n",
    "             Tseq=Tseq)\n",
    "X, Y, VX, VY = [], [], [], []\n",
    "Yaw = []\n",
    "for t in range(100):\n",
    "    x, y, dx, dy = sp.cal(t/99)\n",
    "    X.append(x)\n",
    "    Y.append(y)\n",
    "    VX.append(dx)\n",
    "    VY.append(dy)\n",
    "    Yaw.append(math.atan2(dy, dx))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "27308321",
   "metadata": {},
   "outputs": [],
   "source": [
    "idx= 0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "2b0afe2c",
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'data' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m/tmp/ipykernel_4119058/1902927655.py\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      5\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      6\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 7\u001b[0;31m \u001b[0morig\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdata\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'orig'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0midx\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      8\u001b[0m \u001b[0mrot\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdata\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'rot'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0midx\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      9\u001b[0m \u001b[0mgraph\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdata\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'graph'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0midx\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mNameError\u001b[0m: name 'data' is not defined"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "idx = 2\n",
    "thrd = 2\n",
    "\n",
    "\n",
    "orig = data['orig'][idx]\n",
    "rot = data['rot'][idx]\n",
    "graph = data['graph'][idx]\n",
    "    \n",
    "\n",
    "\n",
    "\n",
    "gt_preds = data['gt_preds'][idx]\n",
    "has_preds = data['has_preds'][idx]\n",
    "for k in range(6):\n",
    "    fig, ax = plt.subplots(1,1, figsize=(20,20))\n",
    "    ax.set_aspect('equal', adjustable='box')\n",
    "#     plt.gca().invert_xaxis()\n",
    "\n",
    "#     plt.gca().invert_yaxis()     \n",
    "\n",
    "    ctrs = graph['ctrs']\n",
    "    ax.plot(ctrs[:, 0], ctrs[:, 1], 'k.')\n",
    "    for a in range(data['gt_preds'][idx].shape[0]):\n",
    "#         if a != 3:\n",
    "#             continue\n",
    "        \n",
    "        mask = has_preds[a]\n",
    "        gt_pred = gt_preds[a][mask]\n",
    "        \n",
    "        hist = data['feats'][idx][a]\n",
    "#         plt.plot(hist[:, 0], hist[:, 1], 'y.')\n",
    "        for l in data['ref_paths'][idx][a]:\n",
    "            lane = torch.matmul(l - data['orig'][idx], rot.T)\n",
    "#             plt.plot(lane[:, 0], lane[:, 1], 'g')\n",
    "#         traj_plot(gt_pred,  rot, orig, ax, 'r.')\n",
    "\n",
    "        pred_traj = results[idx][a, k, :, :]\n",
    "#         for i in range(1, pred_traj.shape[0]):\n",
    "#             if dis(pred_traj[i][0], pred_traj[i - 1][0], pred_traj[i][1], pred_traj[i - 1][1]) < thrd:\n",
    "#                 results[idx][a, k][i] = results[idx][a, k][i - 1]\n",
    "#         traj_plot(pred_traj, rot, orig, ax, 'b.')\n",
    "        print(output['cls'][idx][a][k])\n",
    "        \n",
    "        \n",
    "        traj = torch.matmul(output['reg'][idx][a][k].cpu() - data['orig'][idx], rot.T)\n",
    "        diff = traj[1:] - traj[:-1]\n",
    "#         S_pred = torch.sum(torch.sqrt(torch.sum(diff ** 2, axis=1)))\n",
    "\n",
    "        lane_pts = torch.matmul(output['lane_pts'][idx][a][k].cpu() - data['orig'][idx], rot.T)\n",
    "\n",
    "#         min_final_dis = float('inf')\n",
    "#         min_final_lane = None\n",
    "#         min_final_pt = None\n",
    "#         for ref in data['ref_paths'][idx][a]:\n",
    "#             if ref is None:\n",
    "#                 continue\n",
    "            \n",
    "#             Lane = torch.matmul(ref - data['orig'][idx], rot.T)\n",
    "#             map_car0 = Lane - traj[-1]\n",
    "#             lane_end_id = torch.argmin(torch.sum(map_car0 ** 2, 1))\n",
    "\n",
    "#             final_pt = Lane[lane_end_id]\n",
    "        \n",
    "#             final_dis = torch.sqrt(torch.sum((final_pt - traj[-1]) ** 2))\n",
    "#             if min_final_dis > final_dis:\n",
    "#                 min_final_dis = final_dis\n",
    "#                 min_final_lane = Lane\n",
    "#                 min_final_pt = final_pt\n",
    "            \n",
    "        \n",
    "        traj_plot(gt_pred,  rot, orig, ax, 'r.')\n",
    "        traj_plot(pred_traj, rot, orig, ax, 'b.')\n",
    "        plt.plot(hist[:, 0], hist[:, 1], 'y.')\n",
    "\n",
    "#         plt.scatter(min_final_pt[0], min_final_pt[1], s=150)\n",
    "        plt.scatter(lane_pts[:,0], lane_pts[:,1], s=150)\n",
    "        \n",
    "#     plt.savefig('/home/user/Projects/LaneGCN_modified/results/remote_results/0603_5s/' + str(idx) + '_' + str(k))\n",
    "#     plt.savefig('/home/user/Logs/0826/' + str(idx) + '_' + str(k))\n",
    "    plt.show()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ff36ba57",
   "metadata": {},
   "outputs": [],
   "source": [
    "traj_interval_vel"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "id": "a4e13a32",
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'data' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m/tmp/ipykernel_4119058/4259752269.py\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      5\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      6\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 7\u001b[0;31m \u001b[0morig\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdata\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'orig'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0midx\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      8\u001b[0m \u001b[0mrot\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdata\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'rot'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0midx\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      9\u001b[0m \u001b[0mgraph\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdata\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'graph'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0midx\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mNameError\u001b[0m: name 'data' is not defined"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "# idx = 0\n",
    "thrd = 2\n",
    "\n",
    "\n",
    "orig = data['orig'][idx]\n",
    "rot = data['rot'][idx]\n",
    "graph = data['graph'][idx]\n",
    "    \n",
    "\n",
    "\n",
    "\n",
    "gt_preds = data['gt_preds'][idx]\n",
    "has_preds = data['has_preds'][idx]\n",
    "for k in range(6):\n",
    "    fig, ax = plt.subplots(1,1, figsize=(20,20))\n",
    "    ax.set_aspect('equal', adjustable='box')\n",
    "#     plt.gca().invert_xaxis()\n",
    "#     plt.gca().invert_yaxis()     \n",
    "\n",
    "    ctrs = graph['ctrs']\n",
    "    ax.plot(ctrs[:, 0], ctrs[:, 1], 'k.')\n",
    "    for a in range(data['gt_preds'][idx].shape[0]):\n",
    "#         if a != 3:\n",
    "#             continue\n",
    "            \n",
    "        mask = has_preds[a]\n",
    "        gt_pred = gt_preds[a][mask]\n",
    "        \n",
    "        hist = data['feats'][idx][a]\n",
    "\n",
    "        traj_to_show, traj_interval_vel = interval(data, results, idx, a, k)\n",
    "#         sp = spline5(Xseq=traj_interval_vel[:, 0], \n",
    "#                      Yseq=traj_interval_vel[:, 1])\n",
    "# #                      vx=np.cos(traj_interval_vel[:,2]) * traj_interval_vel[:,3] / 10, \n",
    "# #                      vy=np.sin(traj_interval_vel[:,2]) * traj_interval_vel[:,3] / 10)\n",
    "        \n",
    "#         X, Y, VX, VY = [], [], [], []\n",
    "#         for t in range(100):\n",
    "#             x, y, dx, dy = sp.cal(t/99)\n",
    "#             X.append(x)\n",
    "#             Y.append(y)\n",
    "#             VX.append(dx)\n",
    "#             VY.append(dy)\n",
    "            \n",
    "\n",
    "#         lane_pts = torch.matmul(output['lane_pts'][idx][a][k].cpu() - data['orig'][idx], rot.T)\n",
    "#         plt.plot(lane_pts[:, 0], lane_pts[:, 1], 'g-')\n",
    "\n",
    "        \n",
    "        plt.plot(hist[:, 0], hist[:, 1], 'y.')\n",
    "        traj_plot(gt_preds[a],  rot, orig, ax, 'r.')\n",
    "#         traj_plot(traj_to_show, rot, orig, ax, 'g.')\n",
    "        \n",
    "        pred_traj = np.insert(results[idx][a, k, :, :], 2, values=None, axis=1)\n",
    "#         traj_plot(traj_interval_vel, rot, orig, ax, 'b.')\n",
    "        \n",
    "        traj_plot(traj_to_show, rot, orig, ax, 'b.')\n",
    "\n",
    "#         plt.scatter(lane_pts[:,0], lane_pts[:,1], s=150)\n",
    "        \n",
    "#     plt.savefig('/home/user/Projects/LaneGCN_modified/results/remote_results/0603_5s/' + str(idx) + '_' + str(k))\n",
    "#     plt.savefig('/home/user/Logs/0826/' + str(idx) + '_' + str(k))\n",
    "    plt.show()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c12cfe99",
   "metadata": {},
   "outputs": [],
   "source": [
    "results[idx][a, 1, :, :]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "c2008f27",
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "ename": "FileNotFoundError",
     "evalue": "[Errno 2] No such file or directory: '/home/user/Projects/interaction_gyt/recorded_trackfiles/DR_USA_Intersection_EP0/vehicle_tracks_000.csv'",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mFileNotFoundError\u001b[0m                         Traceback (most recent call last)",
      "\u001b[0;32m/tmp/ipykernel_4119058/2900600399.py\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m     42\u001b[0m \u001b[0mwidths\u001b[0m \u001b[0;34m=\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     43\u001b[0m \u001b[0mlengths\u001b[0m \u001b[0;34m=\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[0;32m---> 44\u001b[0;31m \u001b[0;32mwith\u001b[0m \u001b[0mopen\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mcsv_file\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0;34m'r'\u001b[0m\u001b[0;34m)\u001b[0m \u001b[0;32mas\u001b[0m \u001b[0mf\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     45\u001b[0m     \u001b[0mreader\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mcsv\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mreader\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mf\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     46\u001b[0m     \u001b[0;32mfor\u001b[0m \u001b[0mrow\u001b[0m \u001b[0;32min\u001b[0m \u001b[0mreader\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mFileNotFoundError\u001b[0m: [Errno 2] No such file or directory: '/home/user/Projects/interaction_gyt/recorded_trackfiles/DR_USA_Intersection_EP0/vehicle_tracks_000.csv'"
     ]
    }
   ],
   "source": [
    "k = 0\n",
    "\n",
    "import matplotlib\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib.axes\n",
    "# from matplotlib.patches import Polygon,Circle\n",
    "from matplotlib.collections import PatchCollection\n",
    "\n",
    "\n",
    "def rect_plot(pos, rot, orig, w, l, ax, c):\n",
    "    rot = rot.numpy()\n",
    "    orig = orig.numpy()    \n",
    "\n",
    "    ax.plot(centerline[:, 0], centerline[:, 1], c)\n",
    "    return centerline\n",
    "\n",
    "def polygon_xy_from_motionstate(ms, width, length):\n",
    "    lowleft = (ms[0] - length / 2., ms[1] - width / 2.)\n",
    "    lowright = (ms[0] + length / 2., ms[1] - width / 2.)\n",
    "    upright = (ms[0] + length / 2., ms[1] + width / 2.)\n",
    "    upleft = (ms[0] - length / 2., ms[1] + width / 2.)\n",
    "    return rotate_around_center(np.array([lowleft, lowright, upright, upleft]), np.array([ms[0], ms[1]]), yaw=ms[2])\n",
    "\n",
    "def polygon_xy_from_motionstate_plot(ms, width, length):\n",
    "    lowleft = (ms[0] - length / 2., ms[1] - width / 2.)\n",
    "    lowright = (ms[0] + length / 2., ms[1] - width / 2.)\n",
    "    upright = (ms[0] + length / 2., ms[1] + width / 2.)\n",
    "    upleft = (ms[0] - length / 2., ms[1] + width / 2.)\n",
    "    \n",
    "    right = (ms[0] + length / 2., ms[1])\n",
    "    ru_shoulder = (ms[0] + length / 4., ms[1] + width / 2.)\n",
    "    rl_shoulder = (ms[0] + length / 4., ms[1] - width / 2.)\n",
    "    return rotate_around_center(np.array([rl_shoulder, right, ru_shoulder, upleft, lowleft]), np.array([ms[0], ms[1]]), yaw=ms[2])\n",
    "\n",
    "def rotate_around_center(pts, center, yaw):\n",
    "    return np.dot(pts - center, np.array([[np.cos(yaw), np.sin(yaw)], [-np.sin(yaw), np.cos(yaw)]])) + center\n",
    "\n",
    "\n",
    "import csv\n",
    "\n",
    "csv_file = '/home/user/Projects/interaction_gyt/recorded_trackfiles/DR_USA_Intersection_EP0/vehicle_tracks_000.csv'\n",
    "widths = {}\n",
    "lengths = {}\n",
    "with open(csv_file, 'r') as f:\n",
    "    reader = csv.reader(f)\n",
    "    for row in reader:\n",
    "        track_id = row[0]\n",
    "        if track_id not in widths:\n",
    "            widths[track_id] = row[10]\n",
    "            lengths[track_id] = row[9]\n",
    "\n",
    "\n",
    "            \n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "# idx = 3\n",
    "\n",
    "\n",
    "orig = data['orig'][idx]\n",
    "rot = data['rot'][idx]\n",
    "graph = data['graph'][idx]\n",
    "gt_preds = data['gt_preds'][idx]\n",
    "has_preds = data['has_preds'][idx]\n",
    "\n",
    "fig, ax = plt.subplots(1,1, figsize=(20, 20))\n",
    "ax.set_aspect('equal', adjustable='box')\n",
    "\n",
    "ctrs = graph['ctrs']\n",
    "ax.plot(ctrs[:, 0], ctrs[:, 1], 'k.')\n",
    "\n",
    "\n",
    "res_final_dis, res_final_lane, res_final_pt = [], [], []\n",
    "entire_trajs = np.zeros([gt_preds.shape[0], 20 + config['pred_size'], 3])\n",
    "for a in range(gt_preds.shape[0]):\n",
    "    width, length = data['shapes'][idx][a]\n",
    "\n",
    "    mask = has_preds[a]\n",
    "    gt_pred = gt_preds[a][mask]\n",
    "\n",
    "    hist = data['feats'][idx][a][:,:2]\n",
    "    pred_traj = traj_plot(results[idx][a, k, :, :], rot, orig)\n",
    "    plt.plot(hist[:, 0], hist[:, 1], 'y.')\n",
    "    traj_plot(gt_pred,  rot, orig, ax, 'r.')\n",
    "    \n",
    "#     sp_hist = spline5(hist.numpy()[:, 0], hist.numpy()[:, 1])\n",
    "#     pred_traj[0] = hist[-1]\n",
    "#     sp_pred = spline5(pred_traj[:, 0], pred_traj[:, 1])\n",
    "\n",
    "#     hist_T = hist.shape[0]\n",
    "#     for time_stamp in range(hist_T):\n",
    "#         x, y, dx, dy = sp_hist.cal(time_stamp / (hist_T - 1))\n",
    "#         entire_trajs[a, time_stamp, 0] = x\n",
    "#         entire_trajs[a, time_stamp, 1] = y\n",
    "#         entire_trajs[a, time_stamp, 2] = math.atan2(dy, dx)\n",
    "        \n",
    "#     for time_stamp in range(config['pred_size']):\n",
    "#         x, y, dx, dy = sp_pred.cal(time_stamp / (config['pred_size'] - 1))\n",
    "#         entire_trajs[a, hist_T + time_stamp, 0] = x\n",
    "#         entire_trajs[a, hist_T + time_stamp, 1] = y\n",
    "#         entire_trajs[a, hist_T + time_stamp, 2] = math.atan2(dy, dx)\n",
    "        \n",
    "    min_final_dis = float('inf')\n",
    "    min_final_lane = None\n",
    "    min_final_pt = None\n",
    "    \n",
    "    for ref in data['ref_paths'][idx][a]:\n",
    "        if ref is None:\n",
    "            continue\n",
    "\n",
    "        Lane = torch.matmul(ref - data['orig'][idx], rot.T)\n",
    "        map_car0 = Lane - pred_traj[-1]\n",
    "        lane_end_id = torch.argmin(torch.sum(map_car0 ** 2, 1))\n",
    "    #             plt.plot(Lane[:, 0], Lane[:, 1], 'g')\n",
    "\n",
    "        final_pt = Lane[lane_end_id]\n",
    "    #             plt.scatter(final_pt[0], final_pt[1], s=150)\n",
    "\n",
    "        final_dis = torch.sqrt(torch.sum((final_pt - pred_traj[-1]) ** 2))\n",
    "    #             print(final_pt, traj[-1], final_dis)\n",
    "        if min_final_dis > final_dis:\n",
    "            min_final_dis = final_dis\n",
    "            min_final_lane = Lane\n",
    "            min_final_pt = final_pt\n",
    "            \n",
    "    res_final_dis.append(min_final_dis)\n",
    "    res_final_lane.append(min_final_lane)\n",
    "    res_final_pt.append(min_final_pt)\n",
    "        \n",
    "    plt.plot(min_final_lane[:, 0], min_final_lane[:, 1], 'g', linewidth=5)\n",
    "    plt.scatter(min_final_pt[0], min_final_pt[1], s=150)\n",
    "\n",
    "# for time_stamp in range(entire_trajs.shape[1]):\n",
    "#     for a in range(gt_preds.shape[0]):\n",
    "#         ms = entire_trajs[a][time_stamp]\n",
    "#         rect = matplotlib.patches.Polygon(polygon_xy_from_motionstate(ms, width, length), closed=True, zorder=20)\n",
    "#         ax.add_patch(rect)\n",
    "\n",
    "    traj_to_show, traj_interval_vel = interval(data, results, idx, a, k)\n",
    "    tmp = traj_to_show\n",
    "    traj_plot(traj_to_show, rot, orig, ax, 'b.')\n",
    "    \n",
    "    tmp[:, :2] = np.matmul(data['rot'][idx].cpu().detach().numpy(), (traj_to_show[:, :2] - data['orig'][idx].cpu().detach().numpy()).T).T\n",
    "    tmp[:, 2] = traj_to_show[:, 2] + data['theta'][idx]\n",
    "    \n",
    "    for time_stamp in range(traj_to_show.shape[0]):\n",
    "        ms = tmp[time_stamp][:3]\n",
    "        rect = matplotlib.patches.Polygon(polygon_xy_from_motionstate(ms, width, length), closed=True, zorder=20)\n",
    "        ax.add_patch(rect)\n",
    "    ax.plot(traj_to_show[:, 0], traj_to_show[:, 1], 'b.')\n",
    "    \n",
    "\n",
    "#     plt.savefig('/home/user/Projects/LaneGCN_modified/results/remote_results/0603_5s/' + str(idx) + '_' + str(k))\n",
    "#     plt.savefig('/home/user/Videos/0603/8s/' + str(idx) + '_' + str(k))\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "13d84d32",
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'data' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m/tmp/ipykernel_4119058/1637910772.py\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m     35\u001b[0m \u001b[0max\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mset_aspect\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m'equal'\u001b[0m\u001b[0;34m,\u001b[0m \u001b[0madjustable\u001b[0m\u001b[0;34m=\u001b[0m\u001b[0;34m'box'\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     36\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m---> 37\u001b[0;31m \u001b[0mfilename\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mdata\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m'filename'\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0midx\u001b[0m\u001b[0;34m]\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0msplit\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0;34m'/'\u001b[0m\u001b[0;34m)\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0;36m1\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     38\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m     39\u001b[0m \u001b[0;32mif\u001b[0m \u001b[0mfilename\u001b[0m\u001b[0;34m[\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0;36m6\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m-\u001b[0m\u001b[0;36m4\u001b[0m\u001b[0;34m]\u001b[0m \u001b[0;34m==\u001b[0m \u001b[0;34m\"P0\"\u001b[0m\u001b[0;34m:\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mNameError\u001b[0m: name 'data' is not defined"
     ]
    },
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 1440x1440 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "import matplotlib\n",
    "import matplotlib.pyplot as plt\n",
    "import matplotlib as mpl\n",
    "import os \n",
    "import pickle\n",
    "\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "idx = 6\n",
    "thrd = 2\n",
    "\n",
    "\n",
    "# img_from_left =  plt.imread(os.path.join(\"/home/user/Logs/0902\", 'EP0_from_left_940_1070_950_1040_mask.jpg'))\n",
    "# img_from_right = plt.imread(os.path.join(\"/home/user/Logs/0902\", 'EP0_from_right_940_1070_950_1040_mask.jpg'))\n",
    "# img_from_up =    plt.imread(os.path.join(\"/home/user/Logs/0902\", 'EP0_from_up_940_1070_950_1040_mask.jpg'))\n",
    "# img_from_bot =   plt.imread(os.path.join(\"/home/user/Logs/0902\", 'EP0_from_bot_940_1070_950_1040_mask.jpg'))\n",
    "\n",
    "with open(os.path.join(\"/home/user/Backup/DRL服务器备份/Logs/0902\", 'EP0_from_left_940_1070_950_1040_mask.pkl'), 'rb') as f:\n",
    "    img_from_left = pickle.load(f, encoding=\"latin1\")\n",
    "with open(os.path.join(\"/home/user/Backup/DRL服务器备份/Logs/0902\", 'EP0_from_right_940_1070_950_1040_mask.pkl'), 'rb') as f:\n",
    "    img_from_right = pickle.load(f, encoding=\"latin1\")  \n",
    "with open(os.path.join(\"/home/user/Backup/DRL服务器备份/Logs/0902\", 'EP0_from_up_940_1070_950_1040_mask.pkl'), 'rb') as f:\n",
    "    img_from_up = pickle.load(f, encoding=\"latin1\")\n",
    "with open(os.path.join(\"/home/user/Backup/DRL服务器备份/Logs/0902\", 'EP0_from_bot_940_1070_950_1040_mask.pkl'), 'rb') as f:\n",
    "    img_from_bot = pickle.load(f, encoding=\"latin1\")    \n",
    "with open(os.path.join(\"/home/user/Backup/DRL服务器备份/Logs/0902\", 'EP0_all_940_1070_950_1040_mask.pkl'), 'rb') as f:\n",
    "    img_all = pickle.load(f, encoding=\"latin1\")   \n",
    "    \n",
    "with open(os.path.join(\"/home/user/Backup/DRL服务器备份/Projects/interaction_gyt\", 'EP0_road_polygon.pkl'), 'rb') as f:\n",
    "    road_polygons = pickle.load(f, encoding=\"latin1\")   \n",
    "        \n",
    "\n",
    "fig, ax = plt.subplots(1,1, figsize=(20, 20))\n",
    "ax.set_aspect('equal', adjustable='box')\n",
    "\n",
    "filename = data['filename'][idx].split('/')[-1]\n",
    "\n",
    "if filename[-6:-4] == \"P0\":\n",
    "    use_img = img_EP0\n",
    "    min_x, max_x, min_y, max_y = EP0_bds\n",
    "elif filename[-6:-4] == \"P1\":\n",
    "    use_img = img_EP1    \n",
    "    min_x, max_x, min_y, max_y = EP1_bds\n",
    "elif filename[-6:-4] == \"GL\":\n",
    "    use_img = img_GL\n",
    "    min_x, max_x, min_y, max_y = GL_bds\n",
    "else:\n",
    "    use_img = img_MA\n",
    "    min_x, max_x, min_y, max_y = MA_bds\n",
    "\n",
    "print(min_x, max_x, min_y, max_y)\n",
    "ax.set_xlim([min_x, max_x])\n",
    "ax.set_ylim([min_y, max_y])\n",
    "\n",
    "\n",
    "\n",
    "\n",
    "orig = data['orig'][idx]\n",
    "rot = data['rot'][idx]\n",
    "graph = data['graph'][idx]\n",
    "    \n",
    "\n",
    "gt_preds = data['gt_preds'][idx]\n",
    "has_preds = data['has_preds'][idx]\n",
    "\n",
    "off_road_count = 0\n",
    "for k in range(1):\n",
    "#     plt.gca().invert_xaxis()\n",
    "#     plt.gca().invert_yaxis()     \n",
    "\n",
    "#     ctrs = graph['ctrs']\n",
    "#     nctrs = np.matmul(ctrs, rot) + orig\n",
    "#     for i in range(graph['suc'][0]['u'].shape[0]):\n",
    "#         ax.plot(nctrs[[graph['suc'][0]['u'][i], graph['suc'][0]['v'][i]], 0], nctrs[[graph['suc'][0]['u'][i], graph['suc'][0]['v'][i]], 1], 'k-')\n",
    "    for lb in road_polygons:\n",
    "        ax.plot(lb[:,0], lb[:,1], 'k-', alpha=0.15)\n",
    "\n",
    "#     ax.patches = []\n",
    "\n",
    "    for a in range(data['gt_preds'][idx].shape[0]):\n",
    "#         if a in [2,5,6]:\n",
    "#             continue\n",
    "        if a in [0,2,4,5]:\n",
    "            continue \n",
    "            \n",
    "        mask = has_preds[a]\n",
    "        gt_pred = gt_preds[a][mask]\n",
    "        \n",
    "        gt_hist = data['gt_hist'][idx][a].cpu().detach().numpy()\n",
    "        \n",
    "        traj_to_show, traj_interval_vel = interval(data, results, idx, a, k)\n",
    "            \n",
    "            \n",
    "        origin_yaw = gt_hist[0, 2] % (2 * np.pi)\n",
    "        \n",
    "#         if np.pi / 4 <= origin_yaw and origin_yaw < np.pi * 3 / 4:\n",
    "#             use_img = img_from_bot\n",
    "#             tag = \"bot\"\n",
    "#         elif np.pi * 3 / 4 <= origin_yaw and origin_yaw < np.pi * 5 / 4:\n",
    "#             use_img = img_from_right\n",
    "#             tag = \"right\"\n",
    "#         elif np.pi * 5 / 4 <= origin_yaw and origin_yaw < np.pi * 7 / 4:\n",
    "#             use_img = img_from_up\n",
    "#             tag = \"up\"\n",
    "#         else:\n",
    "#             use_img = img_from_left \n",
    "#             tag = \"left\"\n",
    "        \n",
    "#         use_img = img_all\n",
    "\n",
    "        plt.plot(traj_to_show[:,0], traj_to_show[:,1], color=\"deepskyblue\", marker=\".\")        \n",
    "\n",
    "        flag_off_road = False\n",
    "        for t in range(traj_to_show.shape[0]):\n",
    "            x, y, yaw, vx, vy = traj_to_show[t]\n",
    "            x = min(max_x, max(x, min_x))\n",
    "            y = min(max_y, max(y, min_y))\n",
    "            \n",
    "            grid_x, grid_y = int((x - min_x) / (max_x - min_x + 1) * use_img.shape[1]), int((max_y - y) / (max_y - min_y + 1) * use_img.shape[0])\n",
    "            if use_img[grid_y, grid_x] != 0:\n",
    "#                 print(\"tag from %s agt %d fail at time %d, pos %f, %f, value %d\" %(tag, a, t, x, y, use_img[grid_y, grid_x]))\n",
    "                plt.plot(traj_to_show[t,0], traj_to_show[t,1], 'r.')\n",
    "                flag_off_road = True\n",
    "        \n",
    "        if flag_off_road:\n",
    "            off_road_count += 1\n",
    "            \n",
    "        c = mpl.colors.ListedColormap(['lightgrey', 'white'])\n",
    "        n = mpl.colors.Normalize(vmin=0,vmax=255)        \n",
    "        plt.imshow(use_img, extent=[min_x, max_x, min_y, max_y], cmap=c, norm=n)\n",
    "\n",
    "        plt.plot(gt_hist[:, 0], gt_hist[:, 1], color=\"yellow\", marker=\".\")\n",
    "#         print(tag)\n",
    "    \n",
    "        time_stamp = 70\n",
    "        tmp = np.zeros([3])\n",
    "        tmp[:2] = traj_to_show[time_stamp][:2]\n",
    "        tmp[2] = traj_to_show[time_stamp][2]\n",
    "        \n",
    "\n",
    "        ms = tmp\n",
    "        rect = matplotlib.patches.Polygon(polygon_xy_from_motionstate_plot(ms, width, length), closed=True, zorder=20, edgecolor='black', alpha=0.7)\n",
    "#         if a in [1,3]:\n",
    "#             rect.set_color(\"red\")\n",
    "        ax.add_patch(rect)    \n",
    "#         plt.text(tmp[0], tmp[1]+2, str(a), color=\"r\", size=\"xx-large\")\n",
    "    \n",
    "#     plt.savefig('/home/user/Logs/1102/' + str(idx) + '_GCN', dpi=400)\n",
    "    plt.show()\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "27754ceb",
   "metadata": {},
   "outputs": [],
   "source": [
    "    \n",
    "with open(os.path.join(\"/home/user/Projects/interaction_gyt\", 'EP0_road_polygon.pkl'), 'rb') as f:\n",
    "    road_polygons = pickle.load(f, encoding=\"latin1\")   \n",
    "        "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "id": "27fd4f46",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'/usr/share/fonts/truetype/msttcorefonts/Times_New_Roman.ttf'"
      ]
     },
     "execution_count": 14,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "50e60ab9",
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "# (data['gt_preds'][idx].shape[0])\n",
    "idx = 2\n",
    "k=0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "2960d7a4",
   "metadata": {
    "scrolled": false
   },
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'plt' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "\u001b[0;32m<timed exec>\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n",
      "\u001b[0;32m<timed exec>\u001b[0m in \u001b[0;36mcreate_animation\u001b[0;34m()\u001b[0m\n",
      "\u001b[0;31mNameError\u001b[0m: name 'plt' is not defined"
     ]
    }
   ],
   "source": [
    "%%time\n",
    "import matplotlib.animation as animation\n",
    "from IPython.display import HTML\n",
    "import matplotlib.colors as colors\n",
    "\n",
    "def create_animation():\n",
    "  \"\"\" Creates a Matplotlib animation of the given images.\"\"\"\n",
    "\n",
    "  plt.ioff()\n",
    "  fig, ax = plt.subplots(1,1, figsize=(20, 20))\n",
    "\n",
    "#   plt.gca().invert_xaxis()\n",
    "#   plt.gca().invert_yaxis() \n",
    "    \n",
    "\n",
    "  ax.set_aspect('equal', adjustable='box')\n",
    "    \n",
    "  plt.ion()\n",
    "  \n",
    "  graph = data['graph'][idx]\n",
    "  rot = data['rot'][idx]\n",
    "  orig = data['orig'][idx]\n",
    "  ctrs = graph['ctrs']\n",
    "  ax.plot(ctrs[:, 0], ctrs[:, 1], 'k.')\n",
    "\n",
    "  trajs_array_to_show = []\n",
    "  for a in range(data['gt_preds'][idx].shape[0]):\n",
    "    traj_to_show, traj_interval_vel = interval(data, results, idx, a, k)\n",
    "    traj_plot(results[idx][a, 0, :, :], rot, orig, ax, 'b.')\n",
    "#     traj_plot(traj_to_show, rot, orig, ax, 'b.')\n",
    "    \n",
    "    trajs_array_to_show.append(traj_to_show)\n",
    "#     hist = data['feats'][idx][a][:,:2]\n",
    "#     plt.plot(hist[:, 0], hist[:, 1], 'y.')\n",
    "#     traj_plot(gt_preds[a],  rot, orig, ax, 'r.')\n",
    "    \n",
    "#     plt.plot(res_final_lane[a][:, 0], res_final_lane[a][:, 1], 'g', linewidth=5)\n",
    "#     plt.scatter(res_final_pt[a][0], res_final_pt[a][1], s=150)\n",
    "    \n",
    "\n",
    "  def animate_func(time_stamp):\n",
    "    ax.patches = []\n",
    "    \n",
    "#     for a in new_data['others_track'].keys():\n",
    "    for a in range(len(trajs_array_to_show)):\n",
    "        tmp = np.zeros([3])\n",
    "        tmp[:2] = np.matmul(data['rot'][idx].cpu().detach().numpy(), (trajs_array_to_show[a][time_stamp][:2] - data['orig'][idx].cpu().detach().numpy()).T).T\n",
    "        tmp[2] = trajs_array_to_show[a][time_stamp][2] + data['theta'][idx]\n",
    "        \n",
    "        ms = tmp\n",
    "        rect = matplotlib.patches.Polygon(polygon_xy_from_motionstate_plot(ms, width, length), closed=True, zorder=20)\n",
    "        ax.add_patch(rect)\n",
    "\n",
    "\n",
    "  anim = animation.FuncAnimation(\n",
    "      fig, animate_func, frames=20+config['pred_size'], interval=100)\n",
    "  plt.close(fig)\n",
    "    \n",
    "  return anim\n",
    "\n",
    "anim = create_animation()\n",
    "HTML(anim.to_html5_video())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "91eaa6e9",
   "metadata": {},
   "outputs": [],
   "source": [
    "# results[idx][a, 0, :, :]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f84c1170",
   "metadata": {},
   "outputs": [],
   "source": [
    "origin_data['shape']"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8362d461",
   "metadata": {},
   "outputs": [],
   "source": [
    "torch.arange(prediction.shape[0])\n",
    "prediction.get_device()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3154b2d2",
   "metadata": {},
   "outputs": [],
   "source": [
    "from metrics import compute_joint_metrics\n",
    "compute_joint_metrics(prediction, truth, [])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c344dd0a",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "9e0ce9e5",
   "metadata": {},
   "outputs": [
    {
     "ename": "FileNotFoundError",
     "evalue": "[Errno 2] No such file or directory: '/home/user/Downloads/collected_id_pair'",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mFileNotFoundError\u001b[0m                         Traceback (most recent call last)",
      "\u001b[0;32m/tmp/ipykernel_4119058/3201811379.py\u001b[0m in \u001b[0;36m<module>\u001b[0;34m\u001b[0m\n\u001b[1;32m      3\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      4\u001b[0m \u001b[0mcsv_prefix\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;34m'/home/user/Downloads/collected_id_pair'\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n\u001b[0;32m----> 5\u001b[0;31m \u001b[0mcsv_folders\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0mos\u001b[0m\u001b[0;34m.\u001b[0m\u001b[0mlistdir\u001b[0m\u001b[0;34m(\u001b[0m\u001b[0mcsv_prefix\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      6\u001b[0m \u001b[0;34m\u001b[0m\u001b[0m\n\u001b[1;32m      7\u001b[0m \u001b[0mlarger_than_5s\u001b[0m \u001b[0;34m=\u001b[0m \u001b[0;36m0\u001b[0m\u001b[0;34m\u001b[0m\u001b[0;34m\u001b[0m\u001b[0m\n",
      "\u001b[0;31mFileNotFoundError\u001b[0m: [Errno 2] No such file or directory: '/home/user/Downloads/collected_id_pair'"
     ]
    }
   ],
   "source": [
    "import csv\n",
    "from tqdm import tqdm\n",
    "\n",
    "csv_prefix = '/home/user/Downloads/collected_id_pair'\n",
    "csv_folders = os.listdir(csv_prefix)\n",
    "\n",
    "larger_than_5s = 0\n",
    "files_5s = []\n",
    "\n",
    "larger_than_8s = 0\n",
    "files_8s = []\n",
    "\n",
    "larger_than_10s = 0\n",
    "files_10s = []\n",
    "\n",
    "total_count = 0\n",
    "\n",
    "for folder in tqdm(csv_folders):\n",
    "    csv_path = os.path.join(csv_prefix, folder)\n",
    "    for name in os.listdir(csv_path):\n",
    "        csv_file = os.path.join(csv_path, name)\n",
    "#         print(csv_file)\n",
    "\n",
    "        with open(csv_file, 'r') as f:\n",
    "            reader = csv.reader(f)\n",
    "            for i in reader:\n",
    "                total_count += 1\n",
    "#                 print(i[3], i[4], i[5])\n",
    "                if int(i[5]) - int(i[4]) > 5000:\n",
    "                    larger_than_5s += 1\n",
    "                    files_5s.append([i[3:]])\n",
    "                    \n",
    "                if int(i[5]) - int(i[4]) > 8000:\n",
    "                    larger_than_8s += 1\n",
    "                    files_8s.append([i[3:]])\n",
    "                    \n",
    "                if int(i[5]) - int(i[4]) > 10000:\n",
    "                    larger_than_10s += 1\n",
    "                    files_10s.append([i[3:]])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4b985e02",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "0b17a0d1",
   "metadata": {},
   "outputs": [],
   "source": [
    "# from cvxopt  import solvers, matrix\n",
    "import numpy as np\n",
    "from qpsolvers import solve_qp\n",
    "from numpy import array, dot\n",
    "import sympy as sym\n",
    "import math \n",
    "\n",
    "class spline5:\n",
    "    def __init__(self, Xseq, Yseq, yaw=None, Tseq=None, vx=None, vy=None):\n",
    "        self.Xseq = Xseq\n",
    "        self.Yseq = Yseq\n",
    "        self.vx = vx \n",
    "        self.vy = vy \n",
    "        self.Tseq = Tseq if Tseq is not None else range(len(Xseq))\n",
    "        \n",
    "        self.T = config['pred_size']\n",
    "        order = 6\n",
    "        self.order = order\n",
    "        \n",
    "        if len(self.Tseq) == 1:\n",
    "            return\n",
    "        \n",
    "        p = np.zeros([len(self.Tseq) * 2, (order + 1) * 2])\n",
    "        q = np.zeros([len(self.Tseq) * 2, 1])\n",
    "        for i in range(len(self.Tseq)):\n",
    "            t = self.Tseq[i] / (self.T - 1)\n",
    "            for j in range(order + 1):\n",
    "                ttt = np.power(t, j)\n",
    "                p[i][j] = ttt\n",
    "                p[i + len(self.Tseq)][j + order + 1] = ttt\n",
    "            q[i] = Xseq[i]\n",
    "            q[i + len(self.Tseq)] = Yseq[i]\n",
    "\n",
    "            \n",
    "            \n",
    "        bound = 2\n",
    "###### -b <= d2y/d2x <= b\n",
    "        g = np.zeros([len(self.Tseq) * 2, (order + 1) * 2])\n",
    "        h = np.zeros([len(self.Tseq) * 2])\n",
    "        for i in range(len(self.Tseq)):\n",
    "            t = self.Tseq[i] / (self.T - 1)\n",
    "            \n",
    "            for j in range(2, order + 1):\n",
    "                ttt = np.power(t, j-2)\n",
    "                g[i][j]                 = -bound * j * (j-1) * ttt\n",
    "                g[i][j + order + 1]     = j * (j-1) * ttt            \n",
    "                \n",
    "                g[i + len(self.Tseq)][j]        = -bound * j * (j-1) * ttt\n",
    "                g[i + len(self.Tseq)][j + order + 1] = -j * (j-1) * ttt\n",
    "        \n",
    "\n",
    "####### -b <= d2x/d2y <= b\n",
    "        g2 = np.zeros([len(self.Tseq) * 2, (order + 1) * 2])\n",
    "        h2 = np.zeros([len(self.Tseq) * 2])\n",
    "        for i in range(len(self.Tseq)):\n",
    "            t = self.Tseq[i] / (self.T - 1)\n",
    "            \n",
    "            for j in range(2, order + 1):\n",
    "                ttt = np.power(t, j-2)\n",
    "                g2[i][j]                      = j * (j-1) * ttt            \n",
    "                g2[i][j + order + 1]          = -bound * j * (j-1) * ttt\n",
    "                \n",
    "                g2[i + len(self.Tseq)][j]             = -j * (j-1) * ttt\n",
    "                g2[i + len(self.Tseq)][j + order + 1] = -bound * j * (j-1) * ttt\n",
    "        \n",
    "\n",
    "####### \n",
    "        \n",
    "\n",
    "\n",
    "        \n",
    "#         np.set_printoptions(precision=2)\n",
    "#         print('p', p)\n",
    "#         P = np.dot(p.T, p) + np.dot(g.T, g) * 0.001 + np.eye((order + 1) * 2) * 0.0001\n",
    "        P = np.dot(p.T, p) + np.eye((order + 1) * 2) * 0.0001\n",
    "        Q = -np.dot(q.T, p)\n",
    "            \n",
    "        Q = Q.reshape(((order + 1) * 2,))\n",
    "        \n",
    "        A = np.zeros([5, (order + 1) * 2])\n",
    "        B = np.zeros([A.shape[0], 1])\n",
    "        \n",
    "        A[0][0] = 1\n",
    "        A[1][order + 1] = 1\n",
    "        B[0] = Xseq[0]\n",
    "        B[1] = Yseq[0]\n",
    "        \n",
    "        if vx is not None and vy is not None:\n",
    "            A[2][1] = 1\n",
    "            B[2] = vx \n",
    "            A[3][1 + order + 1] = 1\n",
    "            B[3] = vy \n",
    "            \n",
    "        if yaw is not None:\n",
    "            A[-1][1] = math.tan(yaw) \n",
    "            A[-1][1+order+1] = -1\n",
    "\n",
    "            B[-1] = 0\n",
    "            \n",
    "        B = B.reshape((A.shape[0],))\n",
    "        \n",
    "        \n",
    "        G = np.concatenate([g, g2])\n",
    "        H = np.concatenate([h, h2])\n",
    "                \n",
    "#         self.sol = solve_qp(P=P, q=Q)\n",
    "        self.sol = solve_qp(P=P, q=Q, A=A, b=B)\n",
    "#         self.sol = solve_qp(P=P, q=Q, A=A, b=B, G=G, h=H)\n",
    "\n",
    "\n",
    "\n",
    "    def cal(self, t):\n",
    "        if len(self.Tseq) == 1:\n",
    "            return self.Xseq[0], self.Yseq[0], 0, 0\n",
    "            \n",
    "        x = 0\n",
    "        y = 0\n",
    "        order = self.order\n",
    "        dx = 0\n",
    "        dy = 0\n",
    "        \n",
    "        if t >= 0 and t <= 1:\n",
    "            for i in range(order + 1):\n",
    "                ttt = np.power(t, i)\n",
    "                x += self.sol[i] * ttt\n",
    "                y += self.sol[i + order + 1] * ttt\n",
    "\n",
    "            for i in range(1, order + 1):    \n",
    "                ttt = np.power(t, i - 1)\n",
    "                dx += self.sol[i] * i * ttt\n",
    "                dy += self.sol[i + order + 1] * i * ttt\n",
    "        \n",
    "        return x, y, dx, dy \n",
    "\n",
    "    def plot_scatter(self):\n",
    "        X, Y = [], []\n",
    "\n",
    "        T = [i / (self.T - 1) for i in range(self.T)]\n",
    "        for t in T:\n",
    "            x, y, dx, dy = self.cal(t)\n",
    "            X.append(x)\n",
    "            Y.append(y)\n",
    "\n",
    "            \n",
    "        plt.scatter(T, X)\n",
    "        plt.scatter([ts / (self.T - 1) for ts in (self.Tseq)], self.Xseq, color='red')\n",
    "        plt.show()        \n",
    "#     \n",
    "        plt.scatter(T, Y)\n",
    "        plt.scatter([ts / (self.T - 1) for ts in (self.Tseq)], self.Yseq, color='red')\n",
    "        plt.show()\n",
    "\n",
    "        plt.scatter(X, Y)\n",
    "        plt.scatter(self.Xseq, self.Yseq, color='red')\n",
    "        plt.show()\n",
    "\n",
    "# sp = spline5(Xseq[::1], Yseq[::1])\n",
    "# sp = spline5(Xseq=traj_interval_vel[:, 0], \n",
    "#                      Yseq=traj_interval_vel[:, 1],\n",
    "#                      vx=np.cos(traj_interval_vel[:,2]) * traj_interval_vel[:,3] / 10, \n",
    "#                      vy=np.sin(traj_interval_vel[:,2]) * traj_interval_vel[:,3] / 10)\n",
    "# sp.cal(0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "391961db",
   "metadata": {},
   "outputs": [],
   "source": [
    "order = 5\n",
    "np.diag([1 for i in range(order + 1)] + [0.1 for i in range(order + 1)])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "13404449",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(graph['suc'][0]['u'].shape)\n",
    "print(graph['suc'][0]['u'])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "213b63b2",
   "metadata": {},
   "outputs": [],
   "source": [
    "ego_id = 1\n",
    "data['ctrs'][0][ego_id]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e00dd97a",
   "metadata": {},
   "outputs": [],
   "source": [
    "graph.keys()\n",
    "lane_id = graph['lane_idcs'][ego_graph_id]\n",
    "print(\"start point ctr id\", lane_id)\n",
    "\n",
    "torch.nonzero(graph['lane_idcs']==lane_id)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "53ba2c48",
   "metadata": {},
   "outputs": [],
   "source": [
    "def dfs(cur_graph_id, Lanes, lane, cur_len, S_pred, iter):\n",
    "    graph_u = graph['suc'][0]['u']\n",
    "    graph_v = graph['suc'][0]['v']\n",
    "    \n",
    "    suc_indices = (graph_u==cur_graph_id).nonzero()\n",
    "    if cur_len > S_pred or suc_indices.shape[0] == 0 or iter > 100:\n",
    "        Lanes.append(torch.stack(lane).long())\n",
    "        print(cur_len, S_pred)\n",
    "        print(Lanes[-1])\n",
    "        return \n",
    "    \n",
    "    for suc_id in suc_indices:\n",
    "        lane.append(graph_v[suc_id[0]])\n",
    "        \n",
    "        s_diff = graph['ctrs'][graph_v[suc_id[0]]] - graph['ctrs'][graph_u[suc_id[0]]]\n",
    "        s = torch.sqrt(torch.sum(s_diff ** 2))\n",
    "#         print(s.numpy())\n",
    "        dfs(graph_v[suc_id[0]], Lanes, lane, cur_len + s, S_pred, iter+1)\n",
    "        lane.pop()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1b38ad64",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "02567220",
   "metadata": {},
   "outputs": [],
   "source": [
    "Lanes = []\n",
    "dfs(ego_graph_id, Lanes, [], 0, S_pred=100, iter=0)\n",
    "frenet_lane = graph['ctrs'][Lanes[0]]\n",
    "# print(frenet_lane)\n",
    "# print(graph['ctrs'][ego_graph_id])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f2d3c85a",
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "ego_id = 1\n",
    "k=3\n",
    "\n",
    "pred = results[idx][ego_id][k]\n",
    "diff = np.diff(pred, axis=0)\n",
    "S_pred = np.sum(np.sqrt(np.sum(diff ** 2, axis=1)))\n",
    "\n",
    "print(graph['ctrs'])\n",
    "print(data['ctrs'][idx][ego_id, :])\n",
    "map_car0 = graph['ctrs'] - data['ctrs'][idx][ego_id, :]\n",
    "# map_car0 = graph['ctrs'] - torch.Tensor(np.array([990.57, 983.906]))\n",
    "start = torch.sum(map_car0 ** 2, 1)\n",
    "\n",
    "graph_u = graph['suc'][0]['u']\n",
    "graph_v = graph['suc'][0]['v']\n",
    "\n",
    "ego_graph_id = torch.argmin(start)\n",
    "# ego_graph_id = 1281\n",
    "\n",
    "print(ego_graph_id)\n",
    "graph['ctrs'][ego_graph_id]\n",
    "gt_preds.shape\n",
    "\n",
    "fig, ax = plt.subplots(1,1, figsize=(40,40))\n",
    "ax.set_aspect('equal', adjustable='box')\n",
    "#     plt.gca().invert_xaxis()\n",
    "#     plt.gca().invert_yaxis()     \n",
    "# for a in range(data['gt_preds'][idx].shape[0]):\n",
    "#     mask = has_preds[a]\n",
    "#     gt_pred = gt_preds[a][mask]\n",
    "\n",
    "#     hist = data['feats'][idx][a]\n",
    "# #     plt.plot(hist[:, 0], hist[:, 1], 'y.')\n",
    "# #     traj_plot(gt_pred,  rot, orig, ax, 'r.')\n",
    "\n",
    "#     pred_traj = results[idx][a, k, :, :]\n",
    "traj_plot(results[idx][ego_id][k], rot, orig, ax, 'b.')\n",
    "    \n",
    "traj_plot(gt_preds[ego_id],  rot, orig, ax, 'r.')\n",
    "\n",
    "\n",
    "ctrs = graph['ctrs']\n",
    "ax.plot(ctrs[:, 0], ctrs[:, 1], 'k.')\n",
    "\n",
    "Lanes = []\n",
    "dfs(ego_graph_id, Lanes, [], 0, S_pred, iter=0)\n",
    "for Lane in Lanes:\n",
    "    frenet_lane = graph['ctrs'][Lane]\n",
    "    ax.plot(frenet_lane[:, 0], frenet_lane[:, 1], 'g*')\n",
    "ax.plot([graph['ctrs'][ego_graph_id][0]], [graph['ctrs'][ego_graph_id][1]], 'r*')\n",
    "\n",
    "\n",
    "plt.show()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "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.7.11"
  },
  "varInspector": {
   "cols": {
    "lenName": 16,
    "lenType": 16,
    "lenVar": 40
   },
   "kernels_config": {
    "python": {
     "delete_cmd_postfix": "",
     "delete_cmd_prefix": "del ",
     "library": "var_list.py",
     "varRefreshCmd": "print(var_dic_list())"
    },
    "r": {
     "delete_cmd_postfix": ") ",
     "delete_cmd_prefix": "rm(",
     "library": "var_list.r",
     "varRefreshCmd": "cat(var_dic_list()) "
    }
   },
   "types_to_exclude": [
    "module",
    "function",
    "builtin_function_or_method",
    "instance",
    "_Feature"
   ],
   "window_display": false
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}