jupyter_experiment.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e2510300",
   "metadata": {},
   "outputs": [],
   "source": [
    "# In order to correctly run the notebook\n",
    "#PYTHONPATH=PATH_TO_DIR/lmnav_code_release OPENAI_API_KEY=sk-1  jupyter notebook"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0f5f86d7",
   "metadata": {},
   "outputs": [],
   "source": [
    "import base64\n",
    "import json\n",
    "import networkx\n",
    "import numpy as np\n",
    "import pickle\n",
    "from IPython.display import display, Javascript, HTML\n",
    "\n",
    "import lm_nav\n",
    "from lm_nav.navigation_graph import NavigationGraph\n",
    "from lm_nav import optimal_route, pipeline\n",
    "import gdown"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "119d7a8c",
   "metadata": {},
   "source": [
    "## Select if you are using small or large graph"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2c787a9d",
   "metadata": {},
   "outputs": [],
   "source": [
    "use_large_graph = True"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e78cd2ec",
   "metadata": {},
   "outputs": [],
   "source": [
    "if use_large_graph:\n",
    "    graph_file_gdrive_id = \"186_WuE3caY0ADoaJrGurs9PACLsgSjup\"\n",
    "    all_routes = [\n",
    "        (5, \"Go straight toward the white building. Continue straight passing by a white truck until you reach a stop sign.\"),\n",
    "        (5, \"After passing a white building, take right next to a white truck. Then take left and go towards a square with a large tree. Go further, until you find a stop sign.\"),\n",
    "        (173, \"Start going around a building with a red-black wall and pass by a fire hydrant. Take a right and enter a grove. Continue straight and take a right, when you see a manhole cover. Go forward and left, and look for a trailer.\"),\n",
    "        (108, \"Take a right next to a stop sign. Look for a glass building, after passing by a white car.\"),\n",
    "        (247, \"Follow the road and take the right, you should see a blue semi-truck. Behind the truck, take a right next to an orange traffic cone. Go towards a blue dumpster and take left. Look for a picnic bench.\"),\n",
    "        (70, \"Go towards a white trailer. Then take left and go to the traffic lights. Take left again, and look for a traffic cone.\"),\n",
    "        (215, \"Go straight, passing by a stop sign and a  manhole cover. Next, you will see a disabled Parking spot and a red building.\"),\n",
    "        (103, \"First, you need to find a stop sign. Then take left and right and continue until you reach a square with a tree. Continue first straight, then right, until you find a white truck. The final destination is a white building.\"),\n",
    "        (103, \"Go to a stop sign. Continue straight, passing by a white truck. The final destination is a white building.\"),\n",
    "        (211, \"Go straight, until you find a glass building. Drive to a white car nearby. Drive to reach a stop sign, this is your destination.\")\n",
    "    ]\n",
    "    all_routes_gt = [\n",
    "        [5, 8, 77],\n",
    "        [5, 8, 23, 261, 77],\n",
    "        [173, 160, 150, 191, 129, 45],\n",
    "        [108, 210, 217, 220],\n",
    "        [247, 254, 264, 275],\n",
    "        [70, 39, 34, 257],\n",
    "        [215, 194, 184, 170],\n",
    "        [103, 267, 22, 8],\n",
    "        [103, 16, 8],\n",
    "        [211, 220, 217, 204],\n",
    "    ]\n",
    "    landmarks_cache = eval(\"[['a white building', 'a white truck', 'a stop sign'], ['a white building', 'a white truck', 'a square with a large tree', 'a stop sign'], ['a building with a red-black wall', 'a fire hydrant', 'a grove', 'a manhole cover', 'a trailer'], ['a stop sign', 'a white car', 'a glass building'], ['a blue semi-truck', 'an orange traffic cone', 'a blue dumpster', 'a picnic bench'], ['a white trailer', 'traffic lights', 'a traffic cone'], ['a stop sign', 'a manhole cover', 'a disabled Parking spot', 'a red building'], ['a stop sign', 'a square with a tree', 'a white truck', 'a white building'], ['a stop sign', 'a white truck', 'a white building'], ['a glass building', 'a white car', 'a stop sign']]\")\n",
    "else:\n",
    "    graph_file_gdrive_id = \"1SJiZmFDLcCnBGz2XuOLPewR1hz8ScwHi\"\n",
    "    all_routes = [\n",
    "        (180, \"Go straight towards a stop sign, take left and go until you reach a traffic cone. Take another left and then right going towards a blue box. From there take left and look for a baby stroller.\"),\n",
    "        (215, \"Go towards the blue box, take right and left until you reach a traffic cone. Take left and pass by a semi-truck until you find a big log.\"),\n",
    "        (63, \"Start at a traffic cone. Go towards a cardboard box and a parking lot. Continue driving, take a right, and pass by a picnic table. Take left and look for a stop sign.\"),\n",
    "        (160, \"Take first right towards a picnic table. Next, go to a square with a large tree, and take the left to another picnic table. Keep going until you reach a parking lot.\"),\n",
    "        (61, \"Go straight and take right next to a traffic cone. Go straight until you reach a parking lot. Take left, go through a lawn and look for a blue box.\"),\n",
    "        (219, \"Pass by a blue box and look for a big log. Take right and keep going straight, passing by a traffic cone. Take a right and finish at the parking lot.\"),\n",
    "        (186, \"Look for a traffic cone, take left and go straight until you find a square with a tree. Turn right, pass by a cardboard box and go to a picnic table.\"),\n",
    "        (75, \"Go straight pass a picnic table until you reach a street. Take right, pass by an orange trailer and take next right at an intersection. Next, take a right next to a traffic cone, take the next left, and pass by a baby stroller. Go straight and you will reach a parking lot.\"),\n",
    "        (194, \"Take a left when you see a traffic cone. Go straight passing by a semi-track and take left after you see a big log. Drive to a blue box and continue straight until you find a cardboard box next to a parking lot.\"),\n",
    "        (133, \"Take right at a traffic cone, and go straight until you reach a square with a big tree. Take right next and go straight until you find a baby stroller. Take left and right and look for an intersection.\"),\n",
    "    ]\n",
    "    all_routes_gt = [\n",
    "        [180,188, 224,220, 216],\n",
    "        [215, 220, 226, 194, 134, 131],\n",
    "        [63,75,78,149,157,165],\n",
    "        [160,157, 149,202,38,45,50],\n",
    "        [61, 78, 121],\n",
    "        [219, 131, 182],\n",
    "        [186, 15, 205, 44],\n",
    "        [75, 52, 62, 69, 216, 240],\n",
    "        [194, 134, 131, 220, 240],\n",
    "        [133,138,230,216,63],\n",
    "    ]\n",
    "    landmarks_cache = eval(\"[['a stop sign', 'a traffic cone', 'a blue box', 'a baby stroller'], ['a blue box', 'a traffic cone', 'a semi-truck', 'a big log'], ['a traffic cone', 'a cardboard box', 'a parking lot', 'a picnic table', 'a stop sign'], ['a picnic table', 'a square with a large tree', 'another picnic table', 'a parking lot'], ['a traffic cone', 'a parking lot', 'a lawn', 'a blue box'], ['a blue box', 'a big log', 'a traffic cone', 'a parking lot'], ['a traffic cone', 'a square with a tree', 'a cardboard box', 'a picnic table'], ['a picnic table', 'a street', 'an orange trailer', 'an intersection', 'a traffic cone', 'a baby stroller', 'a parking lot'], ['a traffic cone', 'a semi-track', 'a big log', 'a blue box', 'a cardboard box', 'a parking lot'], ['a traffic cone', 'a square with a big tree', 'a baby stroller', 'an intersection']]\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "13610c06",
   "metadata": {},
   "outputs": [],
   "source": [
    "url = f'https://drive.google.com/uc?id={graph_file_gdrive_id}'\n",
    "gdown.download(url, \"graph.pkl\")\n",
    "graph = NavigationGraph(\"graph.pkl\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "320fc9fe",
   "metadata": {},
   "outputs": [],
   "source": [
    "graph.vert_count"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3fb6afe7",
   "metadata": {},
   "outputs": [],
   "source": [
    "# in order not to query OpenAI API, which incurse cost, we have cached the output of landmark query.\n",
    "# If you want to rerun it, please setup your API KEY in env variable:\n",
    "# e.g.: OPENAI_API_KEY=sk-[real api key here] jupyter notebook\n",
    "# and uncomment the line below:\n",
    "# landmarks_cache = [None] * len(all_routes)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "09b30b93",
   "metadata": {},
   "source": [
    "## Visualize"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "id": "ad0b886b",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/javascript": [
       "require.config({paths: {d3: 'https://d3js.org/d3.v5.min'}});"
      ],
      "text/plain": [
       "<IPython.core.display.Javascript object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/javascript": [
       "// First undefine 'bgraph' so we can easily reload this file.\n",
       "require.undef('bgraph');\n",
       "\n",
       "define('bgraph', ['d3'], function (d3) {\n",
       "\n",
       "    function draw(container, verticies, edges, traversal, supplementary_data, width, height) {\n",
       "        width = width || 600;\n",
       "        height = height || 200;\n",
       "        var svg = d3.select(container).append(\"svg\")\n",
       "            .attr('width', width)\n",
       "            .attr('height', height)\n",
       "            .append(\"g\");\n",
       "\n",
       "        // TODO - create arrows: http://thenewcode.com/1068/Making-Arrows-in-SVG\n",
       "        // svg.append(\"marker\").attr(\"id\", \"arrowhead\").attr(\"orient\", \"auto\").append(\"polygon\").attr(\"positions\", \"0 0, 10 3.5, 0 7\")\n",
       "\n",
       "        var text = d3.select(container).append(\"div\")\n",
       "            .attr(\"class\", \"edges\").text(\"Edges: \")\n",
       "\n",
       "        var lines = svg.selectAll('line').data(edges);\n",
       "        lines.enter()\n",
       "            .append('line')\n",
       "            .attr(\"x1\", function (d, i) { return verticies[d[0]][\"position\"][0]; })\n",
       "            .attr(\"y1\", function (d, i) { return verticies[d[0]][\"position\"][1]; })\n",
       "            .attr(\"x2\", function (d, i) { return verticies[d[1]][\"position\"][0]; })\n",
       "            .attr(\"y2\", function (d, i) { return verticies[d[1]][\"position\"][1]; })\n",
       "            .style(\"stroke\", \"grey\");\n",
       "\n",
       "        var rr = 30;\n",
       "\n",
       "        const traversal_edges = [];\n",
       "        for (let i = 0; i < traversal.length - 1; i++) {\n",
       "            traversal_edges.push([traversal[i][0], traversal[i + 1][0]])\n",
       "        }\n",
       "\n",
       "        var traversal_lines = svg.selectAll('line.trav').data(traversal_edges);\n",
       "        traversal_lines.enter()\n",
       "            .append('line')\n",
       "            .attr('class', \"trav\")\n",
       "            .attr(\"x1\", function (d, i) { return verticies[d[0]][\"position\"][0]; })\n",
       "            .attr(\"y1\", function (d, i) { return verticies[d[0]][\"position\"][1]; })\n",
       "            .attr(\"x2\", function (d, i) { return verticies[d[1]][\"position\"][0]; })\n",
       "            .attr(\"y2\", function (d, i) { return verticies[d[1]][\"position\"][1]; })\n",
       "            .style(\"stroke\", \"#C657E1\");\n",
       "\n",
       "        var circles = svg.selectAll('circle').data(Object.entries(traversal).filter(d => d[1][1] == 0));\n",
       "        circles.enter()\n",
       "            .append('circle')\n",
       "            .attr(\"cx\", function (d, i) { return verticies[d[1][0]][\"position\"][0]; })\n",
       "            .attr(\"cy\", function (d, i) { return verticies[d[1][0]][\"position\"][1]; })\n",
       "            .attr(\"r\", 20)\n",
       "            .attr(\"class\", \"nohighlighted\")\n",
       "            .style(\"opacity\", 0.9)\n",
       "\n",
       "        traversal.forEach( d => {\n",
       "            if (d[1] == \"-1\") {\n",
       "                svg.append('circle')\n",
       "                    .attr('cx', verticies[d[0]][\"position\"][0])\n",
       "                    .attr('cy', verticies[d[0]][\"position\"][1])\n",
       "                    .attr('r', 20)\n",
       "                    .attr('class', \"highlighted\")\n",
       "                    .style(\"opacity\", 0.9)\n",
       "            }\n",
       "        })\n",
       "\n",
       "        var images = svg.selectAll(\"image\").data(Object.entries(verticies));\n",
       "\n",
       "        var sep = \"),(\";\n",
       "\n",
       "        var zoomin_size = 300;\n",
       "\n",
       "        images.enter()\n",
       "            .append(\"svg:image\")\n",
       "            .attr('x', function (d, i) { return d[1][\"position\"][0] - rr / 2; })\n",
       "            .attr(\"y\", function (d, i) { return d[1][\"position\"][1] - rr / 2; })\n",
       "            .attr(\"width\", rr)\n",
       "            .attr(\"height\", rr)\n",
       "            .attr(\"node_name\", function (d, i) { return d[0]; })\n",
       "            .attr(\"xlink:href\", function (d, i) { return \"data:image/png;base64,\" + d[1][\"images\"][0]; })\n",
       "            .on('click', function (d, i) {\n",
       "                // d3.select(\"#xyzxyz\").attr(\"xlink:href\", d3.select(this).attr(\"xlink:href\"));\n",
       "                const images_popup = svg.append(\"svg\")\n",
       "                    .attr(\"x\", d3.select(this).attr(\"x\"))\n",
       "                    .attr(\"y\", d3.select(this).attr(\"y\"))\n",
       "                    .on('click', function () {\n",
       "                        d3.select(this).remove();\n",
       "                    });\n",
       "                var x_pos = 0\n",
       "                var deltaX, deltaY;\n",
       "                var dragHandler = d3.drag()\n",
       "                    .on(\"start\", function () {\n",
       "                        var current = d3.select(this);\n",
       "                        deltaX = current.attr(\"x\") - d3.event.x;\n",
       "                        deltaY = current.attr(\"y\") - d3.event.y;\n",
       "                    })\n",
       "                    .on(\"drag\", function () {\n",
       "                        d3.select(this)\n",
       "                            .attr(\"x\", d3.event.x + deltaX)\n",
       "                            .attr(\"y\", d3.event.y + deltaY);\n",
       "                    });\n",
       "                dragHandler(images_popup);\n",
       "                d[1][\"images\"].forEach(elm => {\n",
       "                    images_popup.append(\"svg:image\")\n",
       "                        .attr(\"width\", zoomin_size)\n",
       "                        .attr(\"height\", zoomin_size)\n",
       "                        .attr(\"x\", x_pos)\n",
       "                        .attr(\"y\", 0)\n",
       "                        .attr(\"xlink:href\", \"data:image/png;base64,\" + elm);\n",
       "                    x_pos += zoomin_size;\n",
       "                });\n",
       "                var node_name = d3.select(this).attr(\"node_name\")\n",
       "                var text_box = images_popup.append(\"text\")\n",
       "                    .attr(\"x\", x_pos+10)\n",
       "                    .attr(\"y\", 30)\n",
       "                text_box.append(\"tspan\").text(node_name)\n",
       "                if (node_name in supplementary_data) {\n",
       "                    supplementary_data[node_name].forEach(elem => {\n",
       "                        text_box.append(\"tspan\").text(elem)\n",
       "                            .attr(\"dy\", \"1.2em\")\n",
       "                            .attr(\"x\", x_pos+10)\n",
       "                    })\n",
       "                }\n",
       "                d3.select(container).select(\"div.edges\")\n",
       "                    .text(d3.select(container).select(\"div.edges\").text() + sep + d3.select(this).attr(\"node_name\"));\n",
       "                if (sep == \",\") {\n",
       "                    sep = \"),(\"\n",
       "                } else {\n",
       "                    sep = \",\"\n",
       "                };\n",
       "            })\n",
       "            .on('mouseout', function () {\n",
       "                d3.select(this)\n",
       "                    .transition('fade').duration(500)\n",
       "                    .attr(\"width\", rr)\n",
       "                    .attr(\"height\", rr);\n",
       "            });\n",
       "    }\n",
       "    return draw;\n",
       "});\n",
       "\n",
       "element.append('<small>&#x25C9; &#x25CB; &#x25EF; Loaded bgraph.js &#x25CC; &#x25CE; &#x25CF;</small>');\n"
      ],
      "text/plain": [
       "<IPython.core.display.Javascript object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/html": [
       "<style>\n",
       ".link {\n",
       "  stroke: #555;\n",
       "  width: 3;\n",
       "}\n",
       "\n",
       ".node text {\n",
       "  pointer-events: none;\n",
       "  font: 10px sans-serif;\n",
       "}\n",
       "\n",
       ".nohighlighted {\n",
       "  fill: #C657E1;\n",
       "}\n",
       "\n",
       ".highlighted {\n",
       "  fill: #72e157;\n",
       "}\n",
       "</style>\n",
       "<script src=\"bgraph.js\"></script>"
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "display(Javascript(\"require.config({paths: {d3: 'https://d3js.org/d3.v5.min'}});\"))\n",
    "display(Javascript(filename=lm_nav.__path__[0]+\"/bgraph.js\"))\n",
    "display(HTML(filename=lm_nav.__path__[0]+\"/base.css.html\"))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "84f554ec",
   "metadata": {},
   "outputs": [],
   "source": [
    "def draw_bgraph(verticies, edges, traversal=[], supplemenatry_data={}, width=600, height=400):\n",
    "    display(Javascript(\"\"\"\n",
    "        (function(element){\n",
    "            require(['bgraph'], function(bgraph) {\n",
    "                bgraph(element.get(0), %s, %s, %s, %s, %d, %d);\n",
    "            });\n",
    "        })(element);\n",
    "    \"\"\" % (json.dumps(verticies), json.dumps(edges), json.dumps(traversal), json.dumps(supplemenatry_data), width, height)))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7059a3d6",
   "metadata": {},
   "outputs": [],
   "source": [
    "vis_ver, vis_edge = graph.json_repr_for_visualization(1000)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6808f795",
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "# This should open an interactive visualization in Javascript, it can take about 20 seconds to load.\n",
    "draw_bgraph(vis_ver,vis_edge, width=1300, height=1300)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "0873f350",
   "metadata": {},
   "source": [
    "## Compute routes"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8301a15a",
   "metadata": {},
   "outputs": [],
   "source": [
    "alpha = 0.0002"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f648a125",
   "metadata": {},
   "outputs": [],
   "source": [
    "# This step can take a few minutes.\n",
    "all_results = [pipeline.full_pipeline(graph, start_node=start, instructions=description, alpha=alpha) if cached_landmarks is None else pipeline.full_pipeline(graph, start_node=start, landmarks=cached_landmarks, alpha=alpha) for ((start, description), cached_landmarks) in zip(all_routes, landmarks_cache)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "edb9715e",
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "# Set inx = 0 .. 9 to inpect result on a particular example.\n",
    "inx = 0\n",
    "print(all_routes[inx])\n",
    "draw_bgraph(vis_ver,vis_edge, all_results[inx][\"walk\"], all_results[inx][\"supplementary_data\"], width=1300, height=1300)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b6c40beb",
   "metadata": {},
   "outputs": [],
   "source": [
    "descriptions_with_walks = [(route_input[1], [a[0] for a in route_output[\"walk\"]])for route_input, route_output in zip(all_routes, all_results)]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "64e5c9a5",
   "metadata": {},
   "source": [
    "## Measure route distances and efficiency"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "11dd477c",
   "metadata": {},
   "source": [
    "### Floyd-Warschal algorithm"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1d12bddb",
   "metadata": {},
   "outputs": [],
   "source": [
    "distance = np.zeros((graph.vert_count,graph.vert_count))\n",
    "distance.fill(1e9)\n",
    "\n",
    "for i in range(graph.vert_count):\n",
    "    distance[i,i] = 0\n",
    "    \n",
    "for u,v in graph._graph.edges():\n",
    "    d = np.linalg.norm(graph._pos[u] - graph._pos[v])\n",
    "    distance[u, v] = d\n",
    "    distance[v, u] = d\n",
    "    \n",
    "for k in range(graph.vert_count):\n",
    "    for i in range(graph.vert_count):\n",
    "        for j in range(graph.vert_count):\n",
    "            if distance[i,j] > distance[i,k] + distance[k,j]:\n",
    "                distance[i,j] = distance[i,k] + distance[k,j]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6c290b34",
   "metadata": {},
   "outputs": [],
   "source": [
    "def path_length(path, distance):\n",
    "    prev = None\n",
    "    res = 0.\n",
    "    for i in path:\n",
    "        if prev is not None and i != prev:\n",
    "            res += distance[prev,i]\n",
    "        prev = i\n",
    "    return res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f4425949",
   "metadata": {},
   "outputs": [],
   "source": [
    "path_length(descriptions_with_walks[3][1], distance)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a61b9314",
   "metadata": {},
   "outputs": [],
   "source": [
    "def image_to_html(img):\n",
    "    b64 = str(base64.b64encode(img))[2:-1]\n",
    "    return f'<img src=\"data:image/png;base64,{b64}\" />'\n",
    "\n",
    "def display_route_landmarks(result):\n",
    "    landmarks = result[\"landmarks\"]\n",
    "    path = result[\"walk\"]\n",
    "    landmarks_on_path = [f for f,s in path if s == -1]\n",
    "    assert(len(landmarks) == len(landmarks_on_path))\n",
    "    html_code = \"\"\n",
    "    for landmark, node in zip(landmarks, landmarks_on_path):\n",
    "        html_code += f\"<h2>{landmark}</h2>\"\n",
    "        html_code += image_to_html(graph._images[node][0])\n",
    "        html_code += image_to_html(graph._images[node][1])\n",
    "    return HTML(html_code)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "95804913",
   "metadata": {},
   "outputs": [],
   "source": [
    "# This method displays landmarks assigned on a route.\n",
    "display_route_landmarks(all_results[4])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3c9559f3",
   "metadata": {},
   "outputs": [],
   "source": []
  },
  {
   "cell_type": "markdown",
   "id": "3b333816",
   "metadata": {},
   "source": [
    "## Measure planning efficiency"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e5474fca",
   "metadata": {},
   "outputs": [],
   "source": [
    "walk_with_data = []\n",
    "for i, r in enumerate(all_results):\n",
    "    walk = [a[0] for a in r[\"walk\"]]\n",
    "    walk_with_data.append({\"walk\": walk, \"d_planning\": path_length(walk, distance), \"dh\": path_length(all_routes_gt[i], distance), \"success\": True, \"description\": all_routes[i][1]})\n",
    "\n",
    "if use_large_graph:\n",
    "    walk_with_data[2][\"success\"] = False\n",
    "    walk_with_data[5][\"success\"] = False\n",
    "else:\n",
    "    walk_with_data[7][\"success\"] = False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fc5d77d1",
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "# Planning Efficiency\n",
    "l2 = [min(1,r[\"dh\"]/r[\"d_planning\"]) for r in walk_with_data]\n",
    "np.average([l2[i] for i in range(len(l2)) if walk_with_data[i][\"success\"]])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f55e050f",
   "metadata": {},
   "source": [
    "# Max likelihood experiments"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "21b410e7",
   "metadata": {},
   "outputs": [],
   "source": [
    "def max_likelihood_selection(start, result, distance):\n",
    "    dist = 0.\n",
    "    prev = start\n",
    "    landmarks = []\n",
    "    for i in range(len(result[\"landmarks\"])):\n",
    "        current = np.argmax(result[\"similarity_matrix\"][:,i])\n",
    "        dist += distance[prev, current]\n",
    "        prev = current\n",
    "        landmarks.append(current)\n",
    "    return dist, landmarks"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c79cfab8",
   "metadata": {},
   "outputs": [],
   "source": [
    "max_likelihood_paths = [max_likelihood_selection(all_routes[i][0], all_results[i], distance) for i in range(10)]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2113e142",
   "metadata": {},
   "outputs": [],
   "source": [
    "inx = 1\n",
    "print(all_routes[inx])\n",
    "draw_bgraph(vis_ver,vis_edge, [(all_routes[inx][0], -1)]+[(int(v),-1) for v in max_likelihood_paths[inx][1]], width=1300, height=1300)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "24a7149b",
   "metadata": {},
   "outputs": [],
   "source": [
    "l = [dh/mlp[0] for dh, mlp in zip([a[\"dh\"] for a in walk_with_data], max_likelihood_paths)]\n",
    "l"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "15c4f300",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.average([l[3]]+[l[9]])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b221acfb",
   "metadata": {},
   "outputs": [],
   "source": [
    "inx = 8\n",
    "[(v,-1) for v in max_likelihood_paths[inx][1]]\n",
    "display_route_landmarks({\"landmarks\": all_results[inx][\"landmarks\"], \"walk\": [(v,-1) for v in max_likelihood_paths[inx][1]]})"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0f49d2d4",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}