Add notebook example for 2D simulator

python/examples/notebook_simulator.ipynb

@@ -0,0 +1,219 @@
+{
+  "cells": [
+  {
+   "attachments": {},
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Simulation of 2D data with g2opy\n",
+    "\n",
+    "## Setup\n",
+    "First we need to import g2opy to be able to run.\n",
+    "We do this by adding the lib folder inside g2o's source to the python path and\n",
+    "afterwards import g2opy"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import os\n",
+    "import sys\n",
+    "\n",
+    "# Adjust path if needed\n",
+    "lib_directory = os.path.join(os.getcwd(), \"../../build/lib\")\n",
+    "print(f\"lib_directory {lib_directory}\")\n",
+    "\n",
+    "if not lib_directory in sys.path:\n",
+    "    sys.path.append(lib_directory)\n",
+    "\n",
+    "import g2opy"
+   ]
+  },
+  {
+   "attachments": {},
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Simulate 2D Data\n",
+    "We first create a simulator and specify its configuration.\n",
+    "Afterwards, we run a simulation."
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "simulator = g2opy.Simulator2D()\n",
+    "simulator.config.has_odom = True\n",
+    "simulator.config.has_pose_sensor = True\n",
+    "simulator.config.world_size = 50.\n",
+    "simulator.config.sim_steps = 500\n",
+    "\n",
+    "simulator.setup()\n",
+    "simulator.simulate()\n",
+    "\n",
+    "print(\"Simulation result\")\n",
+    "print(f\"Number of vertices: {len(simulator.graph().vertices())}\")\n",
+    "print(f\"Number of edges {len(simulator.graph().edges())}\")"
+   ]
+  },
+  {
+   "attachments": {},
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Creating an optimizer and solver\n",
+    "Now we are ready to create an optimizer and a solver.\n",
+    "Furthermore, we add the simulated data into the optimizer."
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "def create_optimizer():\n",
+    "    optimizer = g2opy.SparseOptimizer()\n",
+    "    solver = g2opy.BlockSolverX(g2opy.LinearSolverEigenX())\n",
+    "    solver = g2opy.OptimizationAlgorithmLevenberg(solver)\n",
+    "    optimizer.set_algorithm(solver)\n",
+    "    return optimizer\n",
+    "\n",
+    "\n",
+    "optimizer = create_optimizer()\n",
+    "optimizer.add_graph(simulator.graph())\n",
+    "print(f\"Number of vertices: {len(optimizer.vertices())}\")\n",
+    "print(f\"Number of edges {len(optimizer.edges())}\")"
+   ]
+  },
+  {
+   "attachments": {},
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Visualization of the initial state\n",
+    "First the generic code to visualize the graph by a plotly figure."
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "import plotly.graph_objects as go\n",
+    "\n",
+    "\n",
+    "def plot_slam2d(optimizer, title):\n",
+    "    def edges_coord(edge_ids, vertices_position, dim):\n",
+    "        for e in edge_ids:\n",
+    "            yield vertices_position[e[0]][dim]\n",
+    "            yield vertices_position[e[1]][dim]\n",
+    "            yield None\n",
+    "\n",
+    "    fig = go.Figure()\n",
+    "\n",
+    "    # position of the vertices\n",
+    "    vertices_position = {\n",
+    "        id: v.get_estimate_data()[0:2] for id, v in optimizer.vertices().items()\n",
+    "    }\n",
+    "    fig.add_trace(\n",
+    "        go.Scatter(\n",
+    "            x=[d[0] for d in vertices_position.values()],\n",
+    "            y=[d[1] for d in vertices_position.values()],\n",
+    "            mode=\"markers\",\n",
+    "        )\n",
+    "    )\n",
+    "\n",
+    "    # edge_ids\n",
+    "    edge_ids = [[v.id() for v in e.vertices()[0:2]] for e in optimizer.edges()]\n",
+    "    fig.add_trace(\n",
+    "        go.Scatter(\n",
+    "            x=list(edges_coord(edge_ids, vertices_position, 0)),\n",
+    "            y=list(edges_coord(edge_ids, vertices_position, 1)),\n",
+    "            mode=\"lines\",\n",
+    "            line=dict(width=0.5),\n",
+    "        )\n",
+    "    )\n",
+    "\n",
+    "    fig.update_yaxes(\n",
+    "        scaleanchor=\"x\",\n",
+    "        scaleratio=1,\n",
+    "    )\n",
+    "    fig.update_layout(go.Layout({\"title\": title, \"showlegend\": False}))\n",
+    "\n",
+    "    return fig"
+   ]
+  },
+  {
+   "attachments": {},
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "Below, we can visualize the data"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "plot_slam2d(optimizer, \"Initial state\").show()"
+   ]
+  },
+  {
+   "attachments": {},
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Optimization and visualization\n",
+    "Run the optimization and visualize the result"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "optimizer.vertices()[0].set_fixed(True)\n",
+    "optimizer.initialize_optimization()\n",
+    "optimizer.optimize(30)\n",
+    "\n",
+    "plot_slam2d(optimizer, \"Optimized state\").show()\n"
+   ]
+  }
+ ],
+ "metadata": {
+  "interpreter": {
+   "hash": "31f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6"
+  },
+  "kernelspec": {
+   "display_name": "Python 3.9.7 64-bit",
+   "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.12.3"
+  },
+  "orig_nbformat": 4
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}