estimate_spl.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import os\n",
    "from skimage.io import imread, imsave\n",
    "from skimage.transform import rotate\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib inline"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "from gt_map import GTMap\n",
    "gt_map_file = '/home/kirill/TopoSLAM/GT/2n8kARJN3HM/map_cropped_0_600_300_900.png'\n",
    "gt_map = GTMap(gt_map_file)\n",
    "gt_map_image = imread(gt_map_file)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sys\n",
    "sys.path.append('/home/kirill/TopoSLAM/toposlam_ws/src/simple_toposlam_model/scripts')\n",
    "\n",
    "from localization import Localizer\n",
    "from astar_planner import AStarPlanner\n",
    "from topo_graph import TopologicalGraph\n",
    "from dijkstra import dijkstra"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "from tqdm import tqdm_notebook\n",
    "\n",
    "def path_length(path):\n",
    "        result = 0\n",
    "        for i in range(1, len(path)):\n",
    "            result += np.sqrt((path[i][0] - path[i - 1][0]) ** 2 + (path[i][1] - path[i - 1][1]) ** 2)\n",
    "        return result\n",
    "\n",
    "def get_spl(graph, gt_map, trajectory, N=1000):\n",
    "    astar_planner = AStarPlanner(gt_map.gt_map)\n",
    "    localizer = Localizer(graph, gt_map, publish=False)\n",
    "    free_cells = np.array((gt_map.gt_map == 255).nonzero()).T\n",
    "    values = []\n",
    "    for i in tqdm_notebook(range(N)):\n",
    "        start = trajectory[np.random.randint(trajectory.shape[0])]\n",
    "        start_i = int(start[1] * 20 + 480 - gt_map.start_i)\n",
    "        start_j = int(start[0] * 20 + 480 - gt_map.start_j)\n",
    "        goal = free_cells[np.random.randint(free_cells.shape[0])]\n",
    "        goal_i, goal_j = goal\n",
    "        #print(start_i, start_j, goal_i, goal_j)\n",
    "        shortest_path = astar_planner.create_path((start_i, start_j), (goal_i, goal_j))\n",
    "        if shortest_path is None:\n",
    "            #print('No shortest path from ({}, {}) to ({}, {})!'.format(start_i, start_j, goal_i, goal_j))\n",
    "            continue\n",
    "        if len(shortest_path) == 0:\n",
    "            #print('Empty shortest path from ({}, {}) to ({}, {})!'.format(start_i, start_j, goal_i, goal_j))\n",
    "            continue\n",
    "        localizer.x = start[0]\n",
    "        localizer.y = start[1]\n",
    "        localizer.theta = 0\n",
    "        vertex_ids, rel_poses, dists = localizer.localize()\n",
    "        if len(dists) == 0:\n",
    "            continue\n",
    "        dist_from_start = dists[0]\n",
    "        u = vertex_ids[0]\n",
    "        goal_x = (goal[1] - 480 + gt_map.start_j) / 20\n",
    "        goal_y = (goal[0] - 480 + gt_map.start_i) / 20\n",
    "        localizer.x = goal_x\n",
    "        localizer.y = goal_y\n",
    "        localizer.theta = 0\n",
    "        vertex_ids, rel_poses, dists = localizer.localize()\n",
    "        if len(dists) == 0:\n",
    "            #print('({}, {}) Not localized!'.format(goal_x, goal_y))\n",
    "            continue\n",
    "        dist_to_finish = dists[0]\n",
    "        v = vertex_ids[0]\n",
    "        topo_path, topo_path_length = graph.get_path_with_length(u, v)\n",
    "        #print('Shortest path:', shortest_path)\n",
    "        #print('Topo path:', topo_path)\n",
    "        if u != v:\n",
    "            values.append(path_length(shortest_path) * 0.05 / (dist_from_start + topo_path_length + dist_to_finish))\n",
    "        else:\n",
    "            values.append(1.0)\n",
    "        #print(values[-1])\n",
    "    return np.mean(values)#, shortest_path, topo_path"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Graph of scene 2n8kARJN3HM has 42 nodes and 170 edges\n",
      "Graph of scene 7y3sRwLe3Va has 25 nodes and 92 edges\n",
      "Graph of scene 7y3sRwLe3Va_3 has 20 nodes and 56 edges\n",
      "Graph of scene 7y3sRwLe3Va_4 has 15 nodes and 36 edges\n",
      "Graph of scene D7N2EKCX4Sj has 21 nodes and 60 edges\n",
      "Graph of scene D7N2EKCX4Sj_2 has 24 nodes and 90 edges\n",
      "Graph of scene D7N2EKCX4Sj_3 has 28 nodes and 92 edges\n",
      "Graph of scene E9uDoFAP3SH has 66 nodes and 300 edges\n",
      "Graph of scene E9uDoFAP3SH_2 has 67 nodes and 270 edges\n",
      "Graph of scene JeFG25nYj2p_1 has 47 nodes and 262 edges\n",
      "Graph of scene mJXqzFtmKg4_1 has 71 nodes and 406 edges\n",
      "Graph of scene p5wJjkQkbXX has 83 nodes and 390 edges\n",
      "Graph of scene PuKPg4mmafe has 44 nodes and 188 edges\n",
      "Graph of scene rPc6DW4iMge has 21 nodes and 88 edges\n",
      "Graph of scene rPc6DW4iMge_2 has 22 nodes and 132 edges\n"
     ]
    }
   ],
   "source": [
    "#scene_name = '2n8kARJN3HM'\n",
    "iou_threshold = 0.25\n",
    "scene_names = os.listdir('/home/kirill/TopoSLAM/GT')\n",
    "scene_names.sort(key=lambda s: s.capitalize())\n",
    "for scene_name in scene_names:\n",
    "    gt_map_dir = '/home/kirill/TopoSLAM/GT/{}'.format(scene_name)\n",
    "    gt_map_filename = [x for x in os.listdir(gt_map_dir) if x.startswith('map_cropped_')][0]\n",
    "    gt_map_file = os.path.join(gt_map_dir, gt_map_filename)\n",
    "    gt_map = GTMap(gt_map_file)\n",
    "    gt_map_image = imread(gt_map_file)\n",
    "    graph = TopologicalGraph()\n",
    "    path_to_json = '/home/kirill/TopoSLAM/toposlam_ws/src/simple_toposlam_model/graph_jsons_new/{}_{}.json'.format(scene_name, iou_threshold)\n",
    "    graph.load_from_json(path_to_json)\n",
    "    positions = np.loadtxt('/home/kirill/TopoSLAM/IncrementalTopo/experiment1_large_noise/{}/positions.txt'.format(scene_name))\n",
    "    i = positions.shape[0] - 1\n",
    "    while i > 0 and (positions[i] == positions[i - 1]).all():\n",
    "        i -= 1\n",
    "    positions = positions[:i]\n",
    "    positions_ij = positions * 20 + 480 - [gt_map.start_j, gt_map.start_i]\n",
    "    n_edges = sum([len(x) for x in graph.adj_lists])\n",
    "    print('Graph of scene {} has {} nodes and {} edges'.format(scene_name, len(graph.vertices), n_edges))\n",
    "    \"\"\"\n",
    "    spl = get_spl(graph, gt_map, positions)\n",
    "    \n",
    "    plt.figure(figsize=(18, 18))\n",
    "    plt.imshow(gt_map_image, cmap='gray', alpha=0.5)\n",
    "    for x, y, theta in graph.vertices:\n",
    "        i = y * 20 + 480 - gt_map.start_i\n",
    "        j = x * 20 + 480 - gt_map.start_j\n",
    "        plt.scatter([j], [i], color='r', s=100)\n",
    "    plt.plot(positions_ij[:, 0], positions_ij[:, 1], color='g', lw=3)\n",
    "    for u in range(len(graph.adj_lists)):\n",
    "        for v, _, __ in graph.adj_lists[u]:\n",
    "            x1, y1, theta = graph.vertices[u]\n",
    "            x2, y2, theta = graph.vertices[v]\n",
    "            i1 = y1 * 20 + 480 - gt_map.start_i\n",
    "            j1 = x1 * 20 + 480 - gt_map.start_j\n",
    "            i2 = y2 * 20 + 480 - gt_map.start_i\n",
    "            j2 = x2 * 20 + 480 - gt_map.start_j\n",
    "            plt.plot([j1, j2], [i1, i2], color='b', lw=2, alpha=0.7)\n",
    "    plt.title('Scene name: {}, SPL: {}'.format(scene_name, np.round(spl, 3)), fontsize=24)\n",
    "    plt.savefig('graph_pics/{}_0.25.png'.format(scene_name))\n",
    "    plt.show()\n",
    "    \"\"\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "0.8549255146847718"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "spl"
   ]
  },
  {
   "cell_type": "raw",
   "metadata": {},
   "source": [
    "scene_name = 'E9uDoFAP3SH'\n",
    "gt_map_dir = '/home/kirill/TopoSLAM/GT/{}'.format(scene_name)\n",
    "gt_map_filename = [x for x in os.listdir(gt_map_dir) if x.startswith('old_map_cropped_')][0]\n",
    "gt_map_file = os.path.join(gt_map_dir, gt_map_filename)\n",
    "gt_map_image = imread(gt_map_file)\n",
    "\n",
    "gt_map_image_expanded = np.ones((gt_map_image.shape[0] + 60, gt_map_image.shape[1] + 60), dtype=np.uint8) * 127\n",
    "gt_map_image_expanded[30:-30, 30:-30] = gt_map_image\n",
    "gt_map_image_expanded = rotate(gt_map_image_expanded, -2.9 * 180 / np.pi, cval=127, preserve_range=True)\n",
    "gt_map_image_expanded = gt_map_image_expanded[20:, 40:-60].astype(np.uint8)\n",
    "#gt_map_image_expanded = (gt_map_image_expanded * 255).astype(np.uint8)\n",
    "gt_map_image_expanded[gt_map_image_expanded > 127] = 255\n",
    "gt_map_image_expanded[gt_map_image_expanded < 127] = 0\n",
    "plt.imshow(gt_map_image_expanded, cmap='gray')\n",
    "odd_map_filenames = [x for x in os.listdir(gt_map_dir) if x.startswith('map_cropped_')]\n",
    "for fn in odd_map_filenames:\n",
    "    os.remove(os.path.join(gt_map_dir, fn))\n",
    "imsave('/home/kirill/TopoSLAM/GT/{}/map_cropped_185_575_75_535.png'.format(scene_name), gt_map_image_expanded)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}