diff --git a/gtsam/slam/doc/lago.ipynb b/gtsam/slam/doc/lago.ipynb index aa6d0e98b..00f4c2b60 100644 --- a/gtsam/slam/doc/lago.ipynb +++ b/gtsam/slam/doc/lago.ipynb @@ -16,10 +16,10 @@ }, "source": [ "The `gtsam::lago` namespace provides functions for initializing `Pose2` estimates in a 2D SLAM factor graph.\n", - "LAGO stands for Linear Approximation for Graph Optimization. It leverages the structure of typical 2D SLAM problems to efficiently compute an initial guess, particularly for the orientations, which are often the most challenging part for nonlinear solvers.\n", + "LAGO stands for **Linear Approximation for Graph Optimization**. It leverages the structure of typical 2D SLAM problems to efficiently compute an initial guess, particularly for the orientations, which are often the most challenging part for nonlinear solvers.\n", "\n", "The core idea is:\n", - "1. **Estimate Orientations:** Assume orientations are independent of translations and solve a linear system just for the orientations ($\theta$). This exploits the fact that the orientation part of the `Pose2` `BetweenFactor` error is approximately linear for small errors.\n", + "1. **Estimate Orientations:** Assume orientations are independent of translations and solve a linear system just for the orientations ($\\theta$). This exploits the fact that the orientation part of the `Pose2` `BetweenFactor` error is approximately linear for small errors.\n", "2. **Estimate Translations:** Given the estimated orientations, compute the translations by solving another linear system.\n", "\n", "Key functions:\n", @@ -27,7 +27,9 @@ "* `initialize(graph)`: Computes initial estimates for the full `Pose2` variables (orientations and translations).\n", "* `initialize(graph, initialGuess)`: Corrects only the orientation part of a given `initialGuess` using LAGO.\n", "\n", - "LAGO typically assumes the graph contains a spanning tree of odometry measurements and a prior on the starting pose." + "LAGO typically assumes the graph contains a spanning tree of odometry measurements and a prior on the starting pose.\n", + "\n", + "**Important Note**: LAGO expects integer keys numbered from 0 to n-1, with n the number of poses." ] }, { @@ -41,7 +43,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -50,7 +52,11 @@ }, "outputs": [], "source": [ - "%pip install --quiet gtsam-develop" + "try:\n", + " import google.colab\n", + " %pip install --quiet gtsam-develop\n", + "except ImportError:\n", + " pass # Not running on Colab, do nothing" ] }, { @@ -65,11 +71,8 @@ "import gtsam.utils.plot as gtsam_plot\n", "import matplotlib.pyplot as plt\n", "import numpy as np\n", - "from gtsam import NonlinearFactorGraph, Values, Pose2, Point3, PriorFactorPose2, BetweenFactorPose2\n", - "from gtsam import lago\n", - "from gtsam import symbol_shorthand\n", - "\n", - "X = symbol_shorthand.X" + "from gtsam import NonlinearFactorGraph, Pose2, PriorFactorPose2, BetweenFactorPose2\n", + "from gtsam import lago" ] }, { @@ -92,7 +95,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 8, "metadata": { "id": "example_pipeline_code" }, @@ -101,51 +104,26 @@ "name": "stdout", "output_type": "stream", "text": [ - "Original Factor Graph:\n", - "size: 6\n", "\n", - "Factor 0: PriorFactor on x0\n", - " prior mean: (0, 0, 0)\n", - " noise model: diagonal sigmas [0.1; 0.1; 0.05];\n", + "Initial Estimate from LAGO:\n", "\n", - "Factor 1: BetweenFactor(x0,x1)\n", - " measured: (2, 0, 0)\n", - " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "Values with 5 values:\n", + "Value 0: (gtsam::Pose2)\n", + "(-7.47244713e-17, -6.32592437e-17, -0.00193783525)\n", "\n", - "Factor 2: BetweenFactor(x1,x2)\n", - " measured: (2, 0, 1.57079633)\n", - " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "Value 1: (gtsam::Pose2)\n", + "(1.70434147, -0.00881225307, 0.034656973)\n", "\n", - "Factor 3: BetweenFactor(x2,x3)\n", - " measured: (2, 0, 1.57079633)\n", - " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n" - ] - }, - { - "name": "stdout", - "output_type": "stream", - "text": [ + "Value 2: (gtsam::Pose2)\n", + "(3.40930145, 0.0555625509, 1.64569894)\n", "\n", - "Factor 4: BetweenFactor(x3,x4)\n", - " measured: (2, 0, 1.57079633)\n", - " noise model: diagonal sigmas [0.2; 0.2; 0.1];\n", + "Value 3: (gtsam::Pose2)\n", + "(2.9638596, 2.05327873, 3.10897006)\n", "\n", - "Factor 5: BetweenFactor(x4,x0)\n", - " measured: (2.1, 0.1, 1.62079633)\n", - " noise model: diagonal sigmas [0.25; 0.25; 0.15];\n", + "Value 4: (gtsam::Pose2)\n", + "(0.669190885, 2.11357777, -1.71695927)\n", "\n" ] - }, - { - "ename": "IndexError", - "evalue": "invalid map key", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mIndexError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[3], line 25\u001b[0m\n\u001b[0;32m 22\u001b[0m graph\u001b[38;5;241m.\u001b[39mprint(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mOriginal Factor Graph:\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 24\u001b[0m \u001b[38;5;66;03m# 2. Perform LAGO initialization\u001b[39;00m\n\u001b[1;32m---> 25\u001b[0m initial_estimate_lago \u001b[38;5;241m=\u001b[39m \u001b[43mlago\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43minitialize\u001b[49m\u001b[43m(\u001b[49m\u001b[43mgraph\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 27\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mInitial Estimate from LAGO:\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 28\u001b[0m initial_estimate_lago\u001b[38;5;241m.\u001b[39mprint()\n", - "\u001b[1;31mIndexError\u001b[0m: invalid map key" - ] } ], "source": [ @@ -155,72 +133,117 @@ "# Add a prior on the first pose\n", "prior_mean = Pose2(0.0, 0.0, 0.0)\n", "prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))\n", - "graph.add(PriorFactorPose2(X(0), prior_mean, prior_noise))\n", + "graph.add(PriorFactorPose2(0, prior_mean, prior_noise))\n", "\n", "# Add odometry factors (simulating moving in a square)\n", "odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n", - "graph.add(BetweenFactorPose2(X(0), X(1), Pose2(2.0, 0.0, 0.0), odometry_noise))\n", - "graph.add(BetweenFactorPose2(X(1), X(2), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", - "graph.add(BetweenFactorPose2(X(2), X(3), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", - "graph.add(BetweenFactorPose2(X(3), X(4), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", + "graph.add(BetweenFactorPose2(0, 1, Pose2(2.0, 0.0, 0.0), odometry_noise))\n", + "graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", + "graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", + "graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n", "\n", "# Add a loop closure factor\n", "loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25, 0.15]))\n", "# Ideal loop closure would be Pose2(2.0, 0.0, np.pi/2)\n", "measured_loop = Pose2(2.1, 0.1, np.pi/2 + 0.05)\n", - "graph.add(BetweenFactorPose2(X(4), X(0), measured_loop, loop_noise))\n", - "\n", - "graph.print(\"Original Factor Graph:\\n\")\n", + "graph.add(BetweenFactorPose2(4, 0, measured_loop, loop_noise))\n", "\n", "# 2. Perform LAGO initialization\n", "initial_estimate_lago = lago.initialize(graph)\n", "\n", "print(\"\\nInitial Estimate from LAGO:\\n\")\n", - "initial_estimate_lago.print()\n", - "\n", - "# 3. Visualize the LAGO estimate (optional)\n", + "initial_estimate_lago.print()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The block below visualizes the initial estimate computed by the LAGO algorithm. It uses the `gtsam_plot.plot_pose2` function to plot the poses in 2D space. Each pose is represented with its position and orientation, providing an intuitive way to inspect the initialization results. The visualization helps verify the correctness of the initial guess before proceeding with further optimization." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [ + { + "data": { + "image/png": "iVBORw0KGgoAAAANSUhEUgAAAjcAAAHHCAYAAABDUnkqAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjkuMiwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy8hTgPZAAAACXBIWXMAAA9hAAAPYQGoP6dpAAA7OklEQVR4nO3de1yUZf7/8fcAclLAI+ABT+EhNbU0DDqoZZGZu/7MtNpNbbPcUtO1rdXaPOT3sURlumukbW1YpnmotHNJlrolVh7IQ3kWj+ABFRAREe7fH+xMjhxkdJh7uHk9H495MHPPdd/zuW+nmXfXdd332AzDMAQAAGARPmYXAAAA4E6EGwAAYCmEGwAAYCmEGwAAYCmEGwAAYCmEGwAAYCmEGwAAYCmEGwAAYCmEGwAAYCmEGwCmSk9Pl81m09y5cyvV3mazacqUKS6/zsqVK2Wz2bRy5UqX160KvXr1Uq9evcwuA7Akwg1ggrlz58pms2ndunWVXicmJkY2m02zZ8+usN1///tfDR48WE2bNpW/v7/CwsLUo0cPPf/88zpy5Eip9oZhaN68ebrllltUt25dBQcH65prrtHzzz+vvLy8StU2ZcoU2Ww2HT9+vNL7U5HPP//8sgKMO9j/bcq7rV27ttLb+uWXXzRlyhSlp6dXXcGX4bXXXqt0mASqIz+zCwBwaTt37tRPP/2kli1bav78+XrsscfKbDdp0iRNmzZNrVu31vDhw9W6dWudPXtW69ev1/Tp0/X2229r9+7djvZFRUV64IEHtHjxYt18882aMmWKgoOD9d///ldTp07VkiVL9PXXXysiIqLK9q1FixbKz89XrVq1HMs+//xzJSUllRlw8vPz5edX9R9dzz//vFq1alVqeXR0dKW38csvv2jq1Knq1auXWrZs6fTc8uXLr7TEy/baa6+pYcOGGj58uGk1AFWJcANUA++++67Cw8M1ffp0DRo0SOnp6aW+LBctWqRp06Zp8ODBmjdvnvz9/Z2enzFjhmbMmOG07MUXX9TixYv117/+VS+99JJj+aOPPqrBgwdrwIABGj58uL744osq2zebzabAwMBKt3el7ZXo27evunfvXmXbv/jfB4D7MCwFVAMLFizQoEGDdPfddyssLEwLFiwo1WbSpElq2LCh/vOf/5T5xRkWFubUE5Kfn6+XXnpJbdu2VUJCQqn2/fv317Bhw/Tll1+6NBRj16tXL3Xq1Em//PKLevfureDgYDVt2lQvvviiU7uL59wMHz5cSUlJkuQ0HGR38Zybffv26fHHH1e7du0UFBSkBg0a6N577/XIUNDChQvVrVs3hYSEKDQ0VNdcc43++c9/SioZ3rr33nslSb1793bsh33Oz8VzbuxzghYvXqypU6eqadOmCgkJ0aBBg5Sdna2CggKNGzdO4eHhqlOnjh566CEVFBQ41ZOcnKxbb71V4eHhCggIUIcOHUoNY7Zs2VJbt27VqlWrHDVdWMepU6c0btw4RUVFKSAgQNHR0UpMTFRxcbH7DyBQRei5AbzcDz/8oF27dik5OVn+/v4aOHCg5s+fr2eeecbRZseOHdqxY4dGjBihOnXqVGq73333nU6ePKmxY8eWO8wzdOhQJScn69NPP9UNN9zgcu0nT57UnXfeqYEDB2rw4MF6//339be//U3XXHON+vbtW+Y6I0eO1OHDh5WSkqJ58+Zd8jV++uknrVmzRvfdd5+aNWum9PR0zZ49W7169dIvv/yi4OBgl+uWpOzs7FJziGw2mxo0aCBJSklJ0f3336/bbrtNiYmJkqRff/1V33//vcaOHatbbrlFTzzxhP71r3/pmWee0dVXXy1Jjr/lSUhIUFBQkCZMmKBdu3Zp1qxZqlWrlnx8fHTy5ElNmTJFa9eu1dy5c9WqVStNmjTJse7s2bPVsWNH/e53v5Ofn58++eQTPf744youLtaoUaMkSTNnztSYMWNUp04dPfvss5LkGHY8c+aMevbsqUOHDmnkyJFq3ry51qxZo4kTJyojI0MzZ868rGMJeJwBwOOSk5MNScZPP/10ybajR482oqKijOLiYsMwDGP58uWGJGPjxo2ONh999JEhyZg5c6bTusXFxcaxY8ecboWFhYZhGMbMmTMNScbSpUvLfe0TJ04YkoyBAwdWWOPkyZMNScaxY8ccy3r27GlIMt555x3HsoKCAiMyMtK45557HMv27t1rSDKSk5Mdy0aNGmWU9/EkyZg8ebLj8ZkzZ0q1SU1NLfXa3377rSHJ+PbbbyvcF/u/TVm3gIAAR7uxY8caoaGhxvnz58vd1pIlS8p9zZ49exo9e/YsVV+nTp2Mc+fOOZbff//9hs1mM/r27eu0fmxsrNGiRQunZWUdi/j4eKN169ZOyzp27Oj02nbTpk0zateubezYscNp+YQJEwxfX19j//795ewp4F0YlgK82Pnz57Vo0SINGTLEMTRjH3aYP3++o11OTo4kleq1yc7OVqNGjZxuaWlpkqTc3FxJUkhISLmvb3/Ovn1X1alTR3/84x8dj/39/RUTE6M9e/Zc1vbKEhQU5LhfWFiorKwsRUdHq27dutqwYcNlbzcpKUkpKSlOtwvnHtWtW1d5eXlKSUm5ovovNnToUKfJ1T169JBhGPrTn/7k1K5Hjx46cOCAzp8/71h24bGw9zz17NlTe/bsUXZ29iVfe8mSJbr55ptVr149HT9+3HHr06ePioqKtHr1ajfsIVD1GJYCvNjy5ct17NgxxcTEaNeuXY7lvXv31nvvvafExET5+Pg4Qsjp06ed1q9Tp47jy3f58uVOk4bt69hDTlkqE4Aq0qxZM6f5MpJUr149bdq06bK2V5b8/HwlJCQoOTlZhw4dkmEYjucq84VenpiYmAonFD/++ONavHix+vbtq6ZNm+qOO+7Q4MGDdeedd172a0pS8+bNnR6HhYVJkqKiokotLy4uVnZ2tmOo7Pvvv9fkyZOVmpqqM2fOOLXPzs52bKs8O3fu1KZNm9SoUaMynz969KhL+wKYhXADeDF778zgwYPLfH7VqlXq3bu32rdvL0nasmWL0/N+fn7q06ePJOngwYNOz9nnfmzatEkDBgwoc/v2ENKhQ4fLqt/X17fM5RcGkCs1ZswYJScna9y4cYqNjVVYWJhsNpvuu+++Kp0EGx4errS0NH311Vf64osv9MUXXyg5OVlDhw7V22+/fdnbLe+YXepY7t69W7fddpvat2+vV155RVFRUfL399fnn3+uGTNmVOpYFBcX6/bbb9fTTz9d5vNt27at5F4A5iLcAF4qLy9PH330kYYMGaJBgwaVev6JJ57Q/Pnz1bt3b7Vr105t2rTRsmXLNHPmTNWuXfuS27/ppptUt25dLViwQM8++2yZX57vvPOOJOnuu+++8h1ywcW9PRV5//33NWzYME2fPt2x7OzZszp16lQVVObM399f/fv3V//+/VVcXKzHH39cr7/+up577jlFR0e7tB9X6pNPPlFBQYE+/vhjp96fb7/9tlTb8uq66qqrdPr0aUcgBqor5twAXmrp0qXKy8vTqFGjNGjQoFK3u+++Wx988IHjdOApU6bo+PHjeuSRR1RYWFhqexf3lgQHB+uvf/2rtm/f7jhr5kKfffaZ5s6dq/j4+Ms6U+pK2MNZZQKKr69vqX2bNWuWioqKqqI0h6ysLKfHPj4+6ty5syQ5/k1c2Y8rZQ+nFw/LJScnl2pbu3btMmsaPHiwUlNT9dVXX5V67tSpU07zewBvRs8NYKK33npLX375ZanlY8eO1fz589WgQQPFxcWVue7vfvc7vfHGG/rss880cOBAPfDAA9qyZYsSEhL0448/6r777lOrVq2Ul5enLVu26L333lNISIjq1avn2MaECRO0ceNGJSYmKjU1Vffcc4+CgoL03Xff6d1339XVV199RUMsl6tbt26SSnqn4uPj5evrq/vuu6/MtnfffbfmzZunsLAwdejQQampqfr6668d81Au1xdffKFt27aVWh4XF6fWrVtrxIgROnHihG699VY1a9ZM+/bt06xZs9S1a1fHkF/Xrl3l6+urxMREZWdnKyAgwDEh3N3uuOMOR0/SyJEjdfr0ab3xxhsKDw9XRkaGU9tu3bpp9uzZ+r//+z9FR0crPDxct956q5566il9/PHHuvvuuzV8+HB169ZNeXl52rx5s95//32lp6erYcOGbq8dcDsTz9QCaqyKTjeWZOzbt8/w8/MzHnzwwXK3cebMGSM4ONj4f//v/zktX7lypTFo0CCjcePGRq1atYzQ0FCje/fuxuTJk42MjIxS2ykqKjKSk5ONG2+80QgNDTUCAwONjh07GlOnTjVOnz5dqf0p71Twjh07lmo7bNgwp1OYyzoV/Pz588aYMWOMRo0aGTabzem0cF10KvjJkyeNhx56yGjYsKFRp04dIz4+3ti2bZvRokULY9iwYY527jgV/MI633//feOOO+4wwsPDDX9/f6N58+bGyJEjSx3jN954w2jdurXh6+vr9PrlnQq+ZMmSMuu5+LIBZR3zjz/+2OjcubMRGBhotGzZ0khMTDTeeustQ5Kxd+9eR7vMzEyjX79+RkhIiCHJqY7c3Fxj4sSJRnR0tOHv7280bNjQiIuLM15++WWnU9QBb2YzDDfO7AMAADAZc24AAIClEG4AAIClEG4AAIClEG4AAIClEG4AAIClEG4AAICl1LiL+BUXF+vw4cMKCQnx6KXRAQDA5TMMQ7m5uWrSpIl8fCrum6lx4ebw4cOlfl0XAABUDwcOHFCzZs0qbFPjwk1ISIikkoMTGhpqcjUAAKAycnJyFBUV5fger0iNCzf2oajQ0FDCDQAA1UxlppQwoRgAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFgK4QYAAFiKn9kFAJX13utjdOD4brXtHq821/bRVfWvUqBfoNllAQC8DOEG1cY7vy7Ul/WOS2u/kNZKNtnUom4LtanfRm0btHXc2tRvoxZ1W8jPh7c3ANREfPqj2ui3w1C9YGlnXDvtOJehnIIcpZ9KV/qpdKXsSXFqW8unlq6qf5VT8LHfbxLSRDabzaS9AABUNZthGIbZRXhSTk6OwsLClJ2drdDQULPLQWUVFEjBwVJxsXT4sIzISB3NO6qdJ3ZqR9YO7cja4bi/M2unCooKyt1U7Vq11aZBmzKDT4PgBh7cKQBAZbny/U24QfXw669Shw5SnTpSTo5UQc9LsVGsgzkHfws9WTu140TJ/b0n96rIKCp33fpB9csc5mrToI3q+Nepij0DAFSCK9/fDEuheti5s+Rv27YVBhtJ8rH5qHlYczUPa64+rfs4PVdYVKi9p/aWGXwO5hzUifwT+uHQD/rh0A+lttskpEmZvT2t67VWgF+A23YVAHBlCDeoHnbsKPnbps0VbaaWby1HOLnYmcIz2nViV6nQszNrp46dOabDuYd1OPewVu1b5bSej81HLcJalOrtadugrZqHNZevj+8V1QxUW+vWSS++KAUGSkFBJX8rc/9Sz/vy3xQqRrhB9WAPN21LhxJ3Ca4VrM4RndU5onOp507mn3Sa03Nh8Mk9l6u9p/Zq76m9+mr3V07r+fv666p6V5UKPW0btFVknUgmNsPa9u2Tlixx/3b9/CofhFwJTZdaLyDgkj3H8A6EG1QPFw5LmaBeUD3FNI1RTNMYp+WGYehI3pHfQk/WDu04UXJ/14ldKigq0K/Hf9Wvx38ttc06/nXKHOZq26Ct6gXV89SuAVWnSxdp1izp7FkpP7/kb2Xul7WssPC37Z4/L+Xmltw8zR58XAhQh4IKNSdoq8LrRGjMU+97vuYaiAnFqB6aNpUOH5bWrpV69DC7mkopKi7SgZwDv4WeC87o2ntqr4qN4nLXbRDUoMzQE10/WrX9a3twLwAvUVT0W+BxJRRdadv8fOkKvyZTm0lxI6TWuX7a/XLhpVdAmThbqgKEm2ro9GkpJKTkflaWVL++ufW4wbmic9pzck+ZwedQ7qEK120a0lSxUbFacm8VdPcDcGYYJT1FVxCgvincoduCFquj0Uhbphw1e4+qLc6WgrXs2lXyt2FDSwQbqWQuTvuG7dW+YftSz+Wdy3NMbL4w9OzI2qGs/Cwdyj2kzNOZJlQN1EA2m1SrVsntMv+H+OzOz6UFixXYpLmbi0N5TA03CQkJ+vDDD7Vt2zYFBQUpLi5OiYmJateuXYXrLVmyRM8995zS09PVpk0bJSYm6q677vJQ1fA4N50pVV3U9q+tLpFd1CWyS6nnTuSf0M6snRVeqweAdzl7/qwk8Vt4HmTqr4KvWrVKo0aN0tq1a5WSkqLCwkLdcccdysvLK3edNWvW6P7779fDDz+sjRs3asCAARowYIC2bNniwcrhUR44U6q6qB9UXz2a9VBcVJzZpQCopPzCfElSUK0gkyupOUztufnyyy+dHs+dO1fh4eFav369brnlljLX+ec//6k777xTTz31lCRp2rRpSklJ0auvvqo5c+ZUec0wgclnSgHAlaDnxvNM7bm5WHZ2tiSpfgXzKlJTU9Wnj/NVZ+Pj45WamlqltcFENWxYCoC12MNNkB89N57iNROKi4uLNW7cON14443q1KlTue0yMzMVERHhtCwiIkKZmWVPsCwoKFBBwW8/opiTk+OeguE5DEsBqMbyz5cMS9Fz4zle03MzatQobdmyRQsXLnTrdhMSEhQWFua4RUVFuXX7qGJZWdKJEyX3o6PNrQUALgPDUp7nFeFm9OjR+vTTT/Xtt9+qWbNmFbaNjIzUkSNHnJYdOXJEkZGRZbafOHGisrOzHbcDBw64rW54gH2+TdOmUm0uXgeg+nFMKGZYymNMDTeGYWj06NFaunSpvvnmG7Vq1eqS68TGxmrFihVOy1JSUhQbG1tm+4CAAIWGhjrdUI0wJAWgmqPnxvNMnXMzatQoLViwQB999JFCQkIc82bCwsIUFFSScIcOHaqmTZsqISFBkjR27Fj17NlT06dPV79+/bRw4UKtW7dO//73v03bD1Qhwg2Aao5w43mm9tzMnj1b2dnZ6tWrlxo3buy4LVq0yNFm//79ysjIcDyOi4vTggUL9O9//1tdunTR+++/r2XLllU4CRnVmH1YijOlAFRT9gnFXOfGc0ztuanMz1qtXLmy1LJ7771X9957bxVUBK9Dzw2Aao6eG8/zignFQJkMgwv4Aaj2CDeeR7iB98rIkPLyJF9fqRKTzQHAGzmGpThbymMIN/Be9iGpli0lf39TSwGAy0XPjecRbuC9mG8DwAIIN55HuIH3Yr4NAAvgV8E9j3AD78UPZgKwAHpuPI9wA+/FsBQAC2BCsecRbuCdioqk3btL7hNuAFRj9Nx4HuEG3mnfPqmwUAoIkPgldwDVGOHG8wg38E72IanoaMmHtymA6osJxZ7Htwa8E2dKAbCAouIiFRYXSqLnxpMIN/BOnCkFwAIKigoc9wk3nkO4gXfiTCkAFmAfkpIIN55EuIF3YlgKgAXYJxP7+fjJz8fP5GpqDsINvE9BgZSeXnKfYSkA1RhnSpmDcAPvs3u3ZBhSSIgUEWF2NQBw2biAnzkIN/A+Fw5J2Wzm1gIAV4CeG3MQbuB9OFMKgEXYww3XuPEswg28D2dKAbAI+9lS9Nx4FuEG3odwA8AiGJYyB+EG3sc+54ZhKQDVHBOKzUG4gXfJzZUyMkruE24AVHP03JiDcAPvYu+1adRIqlfP3FoA4AoRbsxBuIF34crEACyEXwQ3B+EG3oXTwAFYCD035iDcwLtwphQAC3GEG1/CjScRbuBdGJYCYCGOs6UYlvIowg28h2FI27eX3GdYCoAFMCxlDsINvEdWlnTqVMn96GhTSwEAd3D8/ALXufEowg28h31IKipKCg42txYAcAP7sBQ9N55FuIH34EwpABbDsJQ5CDfwHpwpBcBiuM6NOQg38B6cKQXAYui5MQfhBt6DYSkAFkO4MQfhBt7BMOi5AWA5/Cq4OQg38A6HD0tnzki+vlKrVmZXAwBuQc+NOQg38A72IalWraRatcytBQDchHBjDsINvANnSgGwIM6WMgfhBt6B+TYALIieG3MQbuAdOFMKgAURbsxBuIF3YFgKgMUYhsHZUiYh3MB8589Le/aU3CfcALCI88XnVWwUS6LnxtMINzDfvn1SYaEUGCg1a2Z2NQDgFvZeG4kJxZ5GuIH57ENS0dGSD29JANZgn28jSQG+ASZWUvPwTQLzHT4s2WwMSQGwFHu4CfANkM1mM7mamsXP7AIAPfyw9Ic/SLm5ZlcCAG7DNW7MQ7iBdwgMLLkBgEVwGrh5GJYCAKAKEG7MQ7gBAKAKcI0b8xBuAACoAvTcmIdwAwBAFSDcmIdwAwBAFeBsKfMQbgAAqAL03JiHU8EBAKgCUWFRGnj1QF0XeZ3ZpdQ4hBsAAKpAn9Z91Kd1H7PLqJFMHZZavXq1+vfvryZNmshms2nZsmUVtl+5cqVsNlupW2ZmpmcKBgAAXs/UcJOXl6cuXbooKSnJpfW2b9+ujIwMxy08PLyKKgQAANWNqcNSffv2Vd++fV1eLzw8XHXr1nV/QQAAoNqrlmdLde3aVY0bN9btt9+u77//3uxyAACAF6lWE4obN26sOXPmqHv37iooKNCbb76pXr166YcfftB115U9G72goEAFBQWOxzk5OZ4qFwAAmKBahZt27dqpXbt2jsdxcXHavXu3ZsyYoXnz5pW5TkJCgqZOneqpEgEAgMmq5bDUhWJiYrRr165yn584caKys7MdtwMHDniwOgAA4GnVquemLGlpaWrcuHG5zwcEBCggIMCDFQEAADOZGm5Onz7t1Ouyd+9epaWlqX79+mrevLkmTpyoQ4cO6Z133pEkzZw5U61atVLHjh119uxZvfnmm/rmm2+0fPlys3YBAAB4GVPDzbp169S7d2/H4/Hjx0uShg0bprlz5yojI0P79+93PH/u3Dk9+eSTOnTokIKDg9W5c2d9/fXXTtsAAAA1m80wDMPsIjwpJydHYWFhys7OVmhoqNnlAACASnDl+7vaTygGAAC4EOEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYCuEGAABYisvh5u2339Znn33mePz000+rbt26iouL0759+9xaHAAAgKtcDjf/+Mc/FBQUJElKTU1VUlKSXnzxRTVs2FB/+ctf3F4gAACAK/xcXeHAgQOKjo6WJC1btkz33HOPHn30Ud14443q1auXu+sDAABwics9N3Xq1FFWVpYkafny5br99tslSYGBgcrPz3dvdQAAAC5yuefm9ttv14gRI3Tttddqx44duuuuuyRJW7duVcuWLd1dHwAAgEtc7rlJSkpSbGysjh07pg8++EANGjSQJK1fv17333+/2wsEAABwhc0wDMPsIjwpJydHYWFhys7OVmhoqNnlAACASnDl+7tSw1KbNm1Sp06d5OPjo02bNlXYtnPnzpWvFAAAwM0qFW66du2qzMxMhYeHq2vXrrLZbLqww8f+2GazqaioqMqKBQAAuJRKhZu9e/eqUaNGjvsAAADeqlLhpkWLFmXev1gNm74DAAC8kMtnSw0fPlx5eXmllqenp+uWW25xS1EAAACXy+Vw8/PPP6tz585KTU11LHv77bfVpUsXNWzY0K3FAQAAuMrli/j9+OOPeuaZZ9SrVy89+eST2rVrl7744gu98soreuSRR6qiRgAAgEpzOdzUqlVLL730koKDgzVt2jT5+flp1apVio2NrYr6AAAAXOLysFRhYaGefPJJJSYmauLEiYqNjdXAgQP1+eefV0V9AAAALnG556Z79+46c+aMVq5cqRtuuEGGYejFF1/UwIED9ac//UmvvfZaVdQJAABQKS733HTv3l1paWm64YYbJJVcwO9vf/ubUlNTtXr1arcXCAAA4Aq3/rZUQUGBAgIC3LW5KsFvSwEAUP24/belynP27FmdO3fOaZm3hxsAAGBtLg9L5eXlafTo0QoPD1ft2rVVr149pxsAAICZXA43Tz/9tL755hvNnj1bAQEBevPNNzV16lQ1adJE77zzTlXUCAAAUGkuh5tPPvlEr732mu655x75+fnp5ptv1t///nf94x//0Pz5813a1urVq9W/f381adJENptNy5Ytu+Q6K1eu1HXXXaeAgABFR0dr7ty5ru4CAACwMJfDzYkTJ9S6dWtJUmhoqE6cOCFJuummm1w+WyovL09dunRRUlJSpdrv3btX/fr1U+/evZWWlqZx48ZpxIgR+uqrr1zbCQAAYFkuTyhu3bq19u7dq+bNm6t9+/ZavHixYmJi9Mknn6hu3boubatv377q27dvpdvPmTNHrVq10vTp0yVJV199tb777jvNmDFD8fHxLr02AACwJpd7bh566CH9/PPPkqQJEyYoKSlJgYGB+stf/qKnnnrK7QVeKDU1VX369HFaFh8f7/QjnhcrKChQTk6O0w0AAFiXyz03f/nLXxz3+/Tpo23btmn9+vWKjo5W586d3VrcxTIzMxUREeG0LCIiQjk5OcrPz1dQUFCpdRISEjR16tQqrQsAAHiPK7rOjSS1aNFCLVq0cEctVWLixIkaP36843FOTo6ioqJMrAgAAFSlKw43nhQZGakjR444LTty5IhCQ0PL7LWRSi4qyIUFAQCoOVyec2Om2NhYrVixwmlZSkqKYmNjTaoIAAB4m0qHm8OHD7v9xU+fPq20tDSlpaVJKjnVOy0tTfv375dUMqQ0dOhQR/s///nP2rNnj55++mlt27ZNr732mhYvXuw0DwgAANRslQ43HTt21IIFC9z64uvWrdO1116ra6+9VpI0fvx4XXvttZo0aZIkKSMjwxF0JKlVq1b67LPPlJKSoi5dumj69Ol68803OQ0cAAA4VPpXwV977TX97W9/05133qnXX39d9evXr+raqgS/Cg4AQPXjyvd3pXtuHn/8cW3atElZWVnq0KGDPvnkkysuFAAAwN1cOluqVatW+uabb/Tqq69q4MCBuvrqq+Xn57yJDRs2uLVAAAAAV7h8Kvi+ffv04Ycfql69evr9739fKtwAAACYyaVk8sYbb+jJJ59Unz59tHXrVjVq1Kiq6gIAALgslQ43d955p3788Ue9+uqrTqdnAwAAeJNKh5uioiJt2rRJzZo1q8p6AAAArkilw01KSkpV1gEAAOAW1ernFwAAAC6FcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACyFcAMAACzFz+wCAABANZSTI23bJtls0vXXm12NE3puAACA6777TurRQxo50uxKSiHcAAAA1zVsWPI3K8vcOspAuAEAAK5r0KDk7/Hj5tZRBsINAABwnT3cnDkjnT1rbi0X8Ypwk5SUpJYtWyowMFA9evTQjz/+WG7buXPnymazOd0CAwM9WC0AAFBYmOTrW3Lfy4amTA83ixYt0vjx4zV58mRt2LBBXbp0UXx8vI4ePVruOqGhocrIyHDc9u3b58GKAQCAbLbfem8IN85eeeUVPfLII3rooYfUoUMHzZkzR8HBwXrrrbfKXcdmsykyMtJxi4iI8GDFZSs2inXq7CmdOnvK7FIAAPAMwk1p586d0/r169WnTx/HMh8fH/Xp00epqanlrnf69Gm1aNFCUVFR+v3vf6+tW7eW27agoEA5OTlOt6pwNO+o6iXWU/3E+lWyfQAAvI6XTio2NdwcP35cRUVFpXpeIiIilJmZWeY67dq101tvvaWPPvpI7777roqLixUXF6eDBw+W2T4hIUFhYWGOW1RUlNv3A4B5DuceVvtX26v9q+3NLgWoeei5cY/Y2FgNHTpUXbt2Vc+ePfXhhx+qUaNGev3118tsP3HiRGVnZztuBw4cqJK6DMOQVDJkBsBzfG2+2p61Xduztjv+OwTgIV4abkz9+YWGDRvK19dXR44ccVp+5MgRRUZGVmobtWrV0rXXXqtdu3aV+XxAQIACAgKuuFYA3imoVpDjfkFRgQL9OHsS8BgvvZCfqT03/v7+6tatm1asWOFYVlxcrBUrVig2NrZS2ygqKtLmzZvVuHHjqiqzUgz9r+dG9NwAnhTk91u4yS/MN7ESoAai56Zs48eP17Bhw9S9e3fFxMRo5syZysvL00MPPSRJGjp0qJo2baqEhARJ0vPPP68bbrhB0dHROnXqlF566SXt27dPI0aMMHM3AJjEz8dPPjYfFRvFyj+fr3qqZ3ZJQM3hpROKTQ83Q4YM0bFjxzRp0iRlZmaqa9eu+vLLLx2TjPfv3y8fn986mE6ePKlHHnlEmZmZqlevnrp166Y1a9aoQ4cOZu2CJObcAGax2WwK8gtSXmEePTeAp3lpz43NqGEz8HJychQWFqbs7GyFhoa6bbuHcg6p2Yxm8vPxU+FzhW7bLoBLa/RSIx0/c1ybH9usTuGdzC4HqDm++066+WYpOlraubNKX8qV7+9qd7aUt2LODWAe+7wbem4AD/PSnhvCDYBqz37G1Nnz3vXjfYDl2cPNqVPS+fOmlnIhwo2bMOcGMI+j5+Y8PTeAR9X/31X5DUM6edLcWi5AuAFQ7dl7bhiWAjzMz6/k18ElrxqaIty4CXNuAPPQcwOYyAsv5Ee4AVDt2a9KTM8NYAIvnFRMuHET5twA5nEMS9FzA3ge4QYA3I9TwQETeeFVigk3bsKcG8A8zLkBTETPDQC4H9e5AUzEhGLrYs4NYB6GpQAT0XMDAO7HhGLARIQb67LPuQHgecy5AUzEhGLrY0Ix4Hlc5wYwEXNurMs+5waA5zEsBZjowmEpL/kuJNy4GROKAc9jQjFgInu4OX9eys01t5b/Idy4CXNuAPPQcwOYKCio5CZ5zdAU4cbNmHMDeJ6954br3AAm8bJJxYQbN2HODWAeR88Nw1KAObxsUjHhxs2YcwN4HqeCAybzsmvdEG7chDk3gHnouQFMRrixNubcAJ7nuM4NPTeAOQg31sScG8A8nAoOmIwJxdbGnBvA8+zDUgVFBSo2ik2uBqiBmFAMAO5l77mROB0cMAXDUtZkn1DMnBvA8+w9NxLhBjAF4QYA3MvPx09+Pn6SmHcDmIJwY032CcXMuQHMwbVuABPZ59wwoRgA3Idr3QAmsvfcnDkjnTV/aJhw4ybMuQHMxbVuABOFhkp+JUPD3jA0RbgBYAlc6wYwkc0m1a9fcp9wYx3MuQHM5RiWoucGMIcXTSom3ACwBHpuAJN50aRiwo2bMOcGMJe954br3AAmoecGANyLU8EBkxFurIc5N4C5OBUcMBnhBgDci54bwGSEG+thzg1gLsd1bui5AczBhGIAcC96bgCT0XNjPcy5AczFnBvAZIQbAHAvem4AkxFurIc5N4C5uM4NYDL7nJtTp6Tz500thXADwBLouQFMVq9eyV/DkE6eNLUUwo2bMOcGMBdzbgCT+flJdeuW3Dd5aIpwA8AS6LkBvICXzLsh3LgJc24Ac3GdG8ALEG4AwH0cw1L03ADmsU8qJtxYA3NuAHM5hqXouQHMY++5MfkqxYQbAJZAzw3gBRiWshbm3ADmsvfccJ0bwEReEm78TH11AHATTgUHzGEYhs4UntHxM8eVFZqnrNZS0NntusnEmgg3bsKcG8BcnAoOXLlio1inzp5S1pksZeVnKetMVklo+d/9rPyS2/Ezx53aFBQV/LaRodItJzdplXm7QbgBYA32nptzRedUVFwkXx9fkysCzHWu6JxO5J8oO6CcydLxfOeAkpWfpRP5J1RsFF/W6/n7+quBb4gaHDut1n6N3Lw3riHcuAlzbgBz2a9zI5XMu6ntX9vEagD3cRr2uTig2JddsNzeq5J7LveyX7OOfx01CGqghsEN1SC4gRoE/e8W/L9l/7t/4bLatWp7zegF4QaAJdiHpaSSoSnCDbyRW4Z9XGCTTfWD6jsFEUdoKSOgNAhqoPpB9RXgF+DmPfcsrwg3SUlJeumll5SZmakuXbpo1qxZiomJKbf9kiVL9Nxzzyk9PV1t2rRRYmKi7rrrLg9WXD5vSa1ATePr46taPrVUWFzIpGJ43NG8o/pu/3dVP+xTyYBiX1Y3sG6NHKI1PdwsWrRI48eP15w5c9SjRw/NnDlT8fHx2r59u8LDw0u1X7Nmje6//34lJCTo7rvv1oIFCzRgwABt2LBBnTp1MmEPStgnFAMwT1CtIBUWFNbsScWGIZ09K+XnS8HBUmDgpdfBFUvLTNM9i++pdPvqPuzj7WyGyd/KPXr00PXXX69XX31VklRcXKyoqCiNGTNGEyZMKNV+yJAhysvL06effupYdsMNN6hr166aM2fOJV8vJydHYWFhys7OVmhoqNv244eDP+iG/9ygVnVbac/YPW7bLoDKi3w5UkfyjujnP/+szhGdzS6nJGicO1cSNMq62UOIO58/e8F1fubPlx54wLz9r0G2Ht2qRz55pMYM+5jBle9vU3tuzp07p/Xr12vixImOZT4+PurTp49SU1PLXCc1NVXjx493WhYfH69ly5aV2b6goEAFBb+NVebk5Fx54WUwfv2l5M6+fVL9+iX37Qm7Jv612aT16wV4UoXXujEMqbCw6sPFxc+b+f+P+TW4B8vDOoZ31JqH15hdBv7H1HBz/PhxFRUVKSIiwml5RESEtm3bVuY6mZmZZbbPzMwss31CQoKmTp3qnoIrUlQkSbIVFUsnT1b963k7uk5hgqCjJyV/KX/YA9IB39Lho/jy5jq4hc0mBQVVfAsMdO/ztWqZt7+AiUyfc1PVJk6c6NTTk5OTo6ioKLe/Tpe4gdreMEp+Nl/p8ajf/m+tpv4FTBB05lxJuNm/R9p1qcYeCBcXBw1CP+ARpoabhg0bytfXV0eOHHFafuTIEUVGRpa5TmRkpEvtAwICFBBQ9WObQaH11bZ7fJW/DoDyvXjjFOWdydZ1MzpLYU3KDx8BAQQNwMJMDTf+/v7q1q2bVqxYoQEDBkgqmVC8YsUKjR49usx1YmNjtWLFCo0bN86xLCUlRbGxsR6oGIA3u23Q02aXAMALmD4sNX78eA0bNkzdu3dXTEyMZs6cqby8PD300EOSpKFDh6pp06ZKSEiQJI0dO1Y9e/bU9OnT1a9fPy1cuFDr1q3Tv//9bzN3AwAAeAnTw82QIUN07NgxTZo0SZmZmeratau+/PJLx6Th/fv3y8fHx9E+Li5OCxYs0N///nc988wzatOmjZYtW2bqNW4AAID3MP06N55WVde5AQAAVceV72+fCp8FAACoZgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUgg3AADAUvzMLsDTDMOQJOXk5JhcCQAAqCz797b9e7wiNS7c5ObmSpKioqJMrgQAALgqNzdXYWFhFbaxGZWJQBZSXFysw4cPKyQkRDabza3bzsnJUVRUlA4cOKDQ0FC3brs64TiU4DiU4DiU4DiU4Dj8hmNRorLHwTAM5ebmqkmTJvLxqXhWTY3rufHx8VGzZs2q9DVCQ0Nr9BvVjuNQguNQguNQguNQguPwG45Ficoch0v12NgxoRgAAFgK4QYAAFgK4caNAgICNHnyZAUEBJhdiqk4DiU4DiU4DiU4DiU4Dr/hWJSoiuNQ4yYUAwAAa6PnBgAAWArhBgAAWArhBgAAWArhBgAAWArhxkVJSUlq2bKlAgMD1aNHD/34448Vtl+yZInat2+vwMBAXXPNNfr88889VGnVcuU4zJ07VzabzekWGBjowWqrxurVq9W/f381adJENptNy5Ytu+Q6K1eu1HXXXaeAgABFR0dr7ty5VV5nVXP1OKxcubLU+8FmsykzM9MzBVeBhIQEXX/99QoJCVF4eLgGDBig7du3X3I9K34+XM6xsOJnxOzZs9W5c2fHheliY2P1xRdfVLiOFd8Prh4Hd70XCDcuWLRokcaPH6/Jkydrw4YN6tKli+Lj43X06NEy269Zs0b333+/Hn74YW3cuFEDBgzQgAEDtGXLFg9X7l6uHgep5MqTGRkZjtu+ffs8WHHVyMvLU5cuXZSUlFSp9nv37lW/fv3Uu3dvpaWlady4cRoxYoS++uqrKq60arl6HOy2b9/u9J4IDw+vogqr3qpVqzRq1CitXbtWKSkpKiws1B133KG8vLxy17Hq58PlHAvJep8RzZo10wsvvKD169dr3bp1uvXWW/X73/9eW7duLbO9Vd8Prh4HyU3vBQOVFhMTY4waNcrxuKioyGjSpImRkJBQZvvBgwcb/fr1c1rWo0cPY+TIkVVaZ1Vz9TgkJycbYWFhHqrOHJKMpUuXVtjm6aefNjp27Oi0bMiQIUZ8fHwVVuZZlTkO3377rSHJOHnypEdqMsPRo0cNScaqVavKbWPVz4eLVeZY1ITPCMMwjHr16hlvvvlmmc/VlPeDYVR8HNz1XqDnppLOnTun9evXq0+fPo5lPj4+6tOnj1JTU8tcJzU11am9JMXHx5fbvjq4nOMgSadPn1aLFi0UFRV1ydRuVVZ8P1yJrl27qnHjxrr99tv1/fffm12OW2VnZ0uS6tevX26bmvJ+qMyxkKz9GVFUVKSFCxcqLy9PsbGxZbapCe+HyhwHyT3vBcJNJR0/flxFRUWKiIhwWh4REVHuXIHMzEyX2lcHl3Mc2rVrp7feeksfffSR3n33XRUXFysuLk4HDx70RMleo7z3Q05OjvLz802qyvMaN26sOXPm6IMPPtAHH3ygqKgo9erVSxs2bDC7NLcoLi7WuHHjdOONN6pTp07ltrPi58PFKnssrPoZsXnzZtWpU0cBAQH685//rKVLl6pDhw5ltrXy+8GV4+Cu90KN+1VweF5sbKxTSo+Li9PVV1+t119/XdOmTTOxMpihXbt2ateuneNxXFycdu/erRkzZmjevHkmVuYeo0aN0pYtW/Tdd9+ZXYrpKnssrPoZ0a5dO6WlpSk7O1vvv/++hg0bplWrVpX7xW5VrhwHd70XCDeV1LBhQ/n6+urIkSNOy48cOaLIyMgy14mMjHSpfXVwOcfhYrVq1dK1116rXbt2VUWJXqu890NoaKiCgoJMqso7xMTEWCIMjB49Wp9++qlWr16tZs2aVdjWip8PF3LlWFzMKp8R/v7+io6OliR169ZNP/30k/75z3/q9ddfL9XWyu8HV47DxS73vcCwVCX5+/urW7duWrFihWNZcXGxVqxYUe7YYWxsrFN7SUpJSalwrNHbXc5xuFhRUZE2b96sxo0bV1WZXsmK7wd3SUtLq9bvB8MwNHr0aC1dulTffPONWrVqdcl1rPp+uJxjcTGrfkYUFxeroKCgzOes+n4oS0XH4WKX/V644inJNcjChQuNgIAAY+7cucYvv/xiPProo0bdunWNzMxMwzAM48EHHzQmTJjgaP/9998bfn5+xssvv2z8+uuvxuTJk41atWoZmzdvNmsX3MLV4zB16lTjq6++Mnbv3m2sX7/euO+++4zAwEBj69atZu2CW+Tm5hobN240Nm7caEgyXnnlFWPjxo3Gvn37DMMwjAkTJhgPPvigo/2ePXuM4OBg46mnnjJ+/fVXIykpyfD19TW+/PJLs3bBLVw9DjNmzDCWLVtm7Ny509i8ebMxduxYw8fHx/j666/N2oUr9thjjxlhYWHGypUrjYyMDMftzJkzjjY15fPhco6FFT8jJkyYYKxatcrYu3evsWnTJmPChAmGzWYzli9fbhhGzXk/uHoc3PVeINy4aNasWUbz5s0Nf39/IyYmxli7dq3juZ49exrDhg1zar948WKjbdu2hr+/v9GxY0fjs88+83DFVcOV4zBu3DhH24iICOOuu+4yNmzYYELV7mU/pfnim33fhw0bZvTs2bPUOl27djX8/f2N1q1bG8nJyR6v291cPQ6JiYnGVVddZQQGBhr169c3evXqZXzzzTfmFO8mZe2/JKd/35ry+XA5x8KKnxF/+tOfjBYtWhj+/v5Go0aNjNtuu83xhW4YNef94OpxcNd7wWYYhuFaXw8AAID3Ys4NAACwFMINAACwFMINAACwFMINAACwFMINAACwFMINAACwFMINAACwFMINgBrLZrNp2bJlZpcBwM0INwBMU1RUpLi4OA0cONBpeXZ2tqKiovTss89W6etnZGSob9++VfoaADyPKxQDMNWOHTvUtWtXvfHGG/rDH/4gSRo6dKh+/vln/fTTT/L39ze5QgDVDT03AEzVtm1bvfDCCxozZowyMjL00UcfaeHChXrnnXcqDDbz5s1T9+7dFRISosjISD3wwAM6evSo4/nnn39eTZo0UVZWlmNZv3791Lt3bxUXF0tyHpY6d+6cRo8ercaNGyswMFAtWrRQQkJC1ew0gCpFuAFgujFjxqhLly568MEH9eijj2rSpEnq0qVLhesUFhZq2rRp+vnnn7Vs2TKlp6dr+PDhjuefffZZtWzZUiNGjJAkJSUlac2aNXr77bfl41P6o+9f//qXPv74Yy1evFjbt2/X/Pnz1bJlS3fuJgAPYVgKgFfYtm2brr76al1zzTXasGGD/Pz8XFp/3bp1uv7665Wbm6s6depIkvbs2aOuXbvq8ccf17/+9S+9+eabeuCBBxzr2Gw2LV26VAMGDNATTzyhrVu36uuvv5bNZnPrvgHwLHpuAHiFt956S8HBwdq7d68OHjx4yfbr169X//791bx5c4WEhKhnz56SpP379zvatG7dWi+//LISExP1u9/9zinYXGz48OFKS0tTu3bt9MQTT2j58uVXvlMATEG4AWC6NWvWaMaMGfr0008VExOjhx9+WBV1Kufl5Sk+Pl6hoaGaP3++fvrpJy1dulRSydyZC61evVq+vr5KT0/X+fPny93mddddp71792ratGnKz8/X4MGDNWjQIPfsIACPItwAMNWZM2c0fPhwPfbYY+rdu7f+85//6Mcff9ScOXPKXWfbtm3KysrSCy+8oJtvvlnt27d3mkxst2jRIn344YdauXKl9u/fr2nTplVYS2hoqIYMGaI33nhDixYt0gcffKATJ05c8T4C8CzCDQBTTZw4UYZh6IUXXpAktWzZUi+//LKefvpppaenl7lO8+bN5e/vr1mzZmnPnj36+OOPSwWXgwcP6rHHHlNiYqJuuukmJScn6x//+IfWrl1b5jZfeeUVvffee9q2bZt27NihJUuWKDIyUnXr1nXn7gLwAMINANOsWrVKSUlJSk5OVnBwsGP5yJEjFRcXV+7wVKNGjTR37lwtWbJEHTp00AsvvKCXX37Z8bxhGBo+fLhiYmI0evRoSVJ8fLwee+wx/fGPf9Tp06dLbTMkJEQvvviiunfvruuvv17p6en6/PPPyzyzCoB342wpAABgKfwvCQAAsBTCDQAAsBTCDQAAsBTCDQAAsBTCDQAAsBTCDQAAsBTCDQAAsBTCDQAAsBTCDQAAsBTCDQAAsBTCDQAAsBTCDQAAsJT/Dx/u9cn8LzaLAAAAAElFTkSuQmCC", + "text/plain": [ + "
" + ] + }, + "metadata": {}, + "output_type": "display_data" + } + ], + "source": [ "plt.figure(1)\n", "for key in initial_estimate_lago.keys():\n", " gtsam_plot.plot_pose2(1, initial_estimate_lago.atPose2(key), 0.5)\n", "plt.title(\"LAGO Initial Estimate\")\n", "plt.axis('equal')\n", - "# plt.show()\n", - "plt.close() # Close plot to prevent display in output\n", + "plt.show()" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "Finally, let's look at `lago.initializeOrientations` to compute the initial orientation estimatesThis solves a linear system, and the solution is represented as a `VectorValues` object, which stores the estimated angles for each pose as a 1-d vector.\n", "\n", - "# 4. Compare orientation initialization\n", + "We compare these orientation estimates with the orientations extracted from the full LAGO initialization (`lago.initialize`)." + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "\n", + "LAGO Orientations (VectorValues):\n", + "VectorValues: 6 elements\n", + " 0: -1.11022302e-16\n", + " 1: -0.008\n", + " 2: 1.55479633\n", + " 3: 3.11759265\n", + " 4: 4.68038898\n", + " 99999999: 0\n", + "Orientations from full LAGO Values:\n", + " 0: -0.0019\n", + " 1: 0.0347\n", + " 2: 1.6457\n", + " 3: 3.1090\n", + " 4: -1.7170\n" + ] + } + ], + "source": [ "initial_orientations = lago.initializeOrientations(graph)\n", "print(\"\\nLAGO Orientations (VectorValues):\")\n", "initial_orientations.print()\n", "\n", "print(\"Orientations from full LAGO Values:\")\n", "for i in range(5):\n", - " print(f\" X({i}): {initial_estimate_lago.atPose2(X(i)).theta():.4f}\")" + " print(f\" {i}: {initial_estimate_lago.atPose2(i).theta():.4f}\")" ] }, { "cell_type": "markdown", - "metadata": { - "id": "notes_header_md" - }, + "metadata": {}, "source": [ - "## Notes" - ] - }, - { - "cell_type": "markdown", - "metadata": { - "id": "notes_desc_md" - }, - "source": [ - "- LAGO provides a good initial guess, especially for orientations, which can significantly help the convergence of nonlinear optimizers.\n", - "- It assumes the graph structure allows for the orientation estimation (typically requires a spanning tree and a prior).\n", - "- The translation estimates are computed based on the fixed, estimated orientations." + "These are not as accurate (the last one is actually fine, it's $2\\pi$ off) but will still be good enough as an initial estimate." ] } ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -234,7 +257,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 50f2f0071..3fdc31b30 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -467,6 +467,7 @@ typedef gtsam::TriangulationFactor> namespace lago { gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true); gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess); + gtsam::VectorValues initializeOrientations(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true); } } // namespace gtsam diff --git a/python/gtsam/tests/test_lago.py b/python/gtsam/tests/test_lago.py index 8ed5dd943..e538d8984 100644 --- a/python/gtsam/tests/test_lago.py +++ b/python/gtsam/tests/test_lago.py @@ -13,7 +13,7 @@ import unittest import numpy as np import gtsam -from gtsam import Point3, Pose2, PriorFactorPose2, Values +from gtsam import BetweenFactorPose2, Point3, Pose2, PriorFactorPose2, Values class TestLago(unittest.TestCase): @@ -33,6 +33,32 @@ class TestLago(unittest.TestCase): estimateLago: Values = gtsam.lago.initialize(graph) assert isinstance(estimateLago, Values) + def test_initialize2(self) -> None: + """Smokescreen to ensure LAGO can be imported and run on toy data stored in a g2o file.""" + # 1. Create a NonlinearFactorGraph with Pose2 factors + graph = gtsam.NonlinearFactorGraph() + + # Add a prior on the first pose + prior_mean = Pose2(0.0, 0.0, 0.0) + prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05])) + graph.add(PriorFactorPose2(0, prior_mean, prior_noise)) + + # Add odometry factors (simulating moving in a square) + odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1])) + graph.add(BetweenFactorPose2(0, 1, Pose2(2.0, 0.0, 0.0), odometry_noise)) + graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, np.pi / 2), odometry_noise)) + graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, np.pi / 2), odometry_noise)) + graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, np.pi / 2), odometry_noise)) + + # Add a loop closure factor + loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25, 0.15])) + # Ideal loop closure would be Pose2(2.0, 0.0, np.pi/2) + measured_loop = Pose2(2.1, 0.1, np.pi / 2 + 0.05) + graph.add(BetweenFactorPose2(4, 0, measured_loop, loop_noise)) + + estimateLago: Values = gtsam.lago.initialize(graph) + assert isinstance(estimateLago, Values) + if __name__ == "__main__": unittest.main()