diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index de55a5bb4..01a28511c 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -355,7 +355,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor { gtsam::Vector error_vector(const gtsam::VectorValues& c) const; double error(const gtsam::VectorValues& c) const; - //Standard Interface + // Standard Interface gtsam::Matrix getA() const; gtsam::Vector getb() const; size_t rows() const; diff --git a/gtsam/slam/KarcherMeanFactor.h b/gtsam/slam/KarcherMeanFactor.h index 6b25dd261..d81d15faa 100644 --- a/gtsam/slam/KarcherMeanFactor.h +++ b/gtsam/slam/KarcherMeanFactor.h @@ -27,13 +27,17 @@ namespace gtsam { /** * Optimize for the Karcher mean, minimizing the geodesic distance to each of - * the given rotations, by constructing a factor graph out of simple + * the given Lie groups elements, by constructing a factor graph out of simple * PriorFactors. */ template -T FindKarcherMean(const std::vector> &rotations); +typename std::enable_if::IsLieGroup, T>::type +FindKarcherMean(const std::vector> &elements); -template T FindKarcherMean(std::initializer_list &&rotations); +/// FindKarcherMean version from initializer list +template +typename std::enable_if::IsLieGroup, T>::type +FindKarcherMean(std::initializer_list &&elements); /** * The KarcherMeanFactor creates a constraint on all SO(n) variables with diff --git a/gtsam/slam/PlanarProjectionFactor.h b/gtsam/slam/PlanarProjectionFactor.h index d53cd0ae1..dc7e53677 100644 --- a/gtsam/slam/PlanarProjectionFactor.h +++ b/gtsam/slam/PlanarProjectionFactor.h @@ -201,7 +201,7 @@ namespace gtsam { const Cal3DS2& calib, const SharedNoiseModel& model = {}) : PlanarProjectionFactorBase(measured), - NoiseModelFactorN(model, landmarkKey, poseKey), + NoiseModelFactorN(model, poseKey, landmarkKey), bTc_(bTc), calib_(calib) {} diff --git a/gtsam/slam/doc/KarcherMeanFactor.ipynb b/gtsam/slam/doc/KarcherMeanFactor.ipynb index 973987286..64cd85506 100644 --- a/gtsam/slam/doc/KarcherMeanFactor.ipynb +++ b/gtsam/slam/doc/KarcherMeanFactor.ipynb @@ -15,12 +15,12 @@ "id": "desc_md" }, "source": [ - "This header provides functionality related to computing and constraining the Karcher mean (or Fréchet mean) of a set of rotations or other manifold values.\n", + "The [KarcherMeanFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/slam/KarcherMeanFactor.h) header provides functionality related to computing and constraining the Karcher mean (or Fréchet mean) of a set of rotations or other manifold values.\n", "The Karcher mean $\\bar{R}$ of a set of rotations $\\{R_i\\}$ is the rotation that minimizes the sum of squared geodesic distances on the manifold:\n", "$$ \\bar{R} = \\arg \\min_R \\sum_i d^2(R, R_i) = \\arg \\min_R \\sum_i || \\text{Log}(R_i^{-1} R) ||^2 $$\n", "\n", "Functions/Classes:\n", - "* `FindKarcherMean(rotations)`: Computes the Karcher mean of a `std::vector` of rotations (or other suitable manifold type `T`). It solves the minimization problem above using a small internal optimization.\n", + "* `FindKarcherMean`: Computes the Karcher mean of a `std::vector` of rotations (or other suitable manifold type `T`). It solves the minimization problem above using a small internal optimization.\n", "* `KarcherMeanFactor`: A factor that enforces a constraint related to the Karcher mean. It does *not* constrain the mean to a specific value. Instead, it acts as a gauge fixing constraint by ensuring that the *sum of tangent space updates* applied to the variables involved sums to zero. This effectively removes the rotational degree of freedom corresponding to simultaneously rotating all variables." ] }, @@ -35,7 +35,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -44,7 +44,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" ] }, { @@ -57,10 +61,8 @@ "source": [ "import gtsam\n", "import numpy as np\n", - "from gtsam import Rot3, FindKarcherMean, KarcherMeanFactorRot3, NonlinearFactorGraph, Values\n", - "from gtsam import symbol_shorthand\n", - "\n", - "R = symbol_shorthand.R" + "from gtsam import Rot3, FindKarcherMeanRot3, KarcherMeanFactorRot3, Values\n", + "from gtsam.symbol_shorthand import R" ] }, { @@ -111,7 +113,7 @@ "rotations.append(Rot3.Yaw(0.12))\n", "\n", "# Compute the Karcher mean\n", - "karcher_mean = FindKarcherMean(rotations)\n", + "karcher_mean = FindKarcherMeanRot3(rotations)\n", "\n", "print(\"Input Rotations (Yaw angles):\")\n", "for r in rotations: print(f\" {r.yaw():.3f}\")\n", @@ -141,7 +143,7 @@ }, { "cell_type": "code", - "execution_count": 9, + "execution_count": 6, "metadata": { "id": "factor_example_code" }, @@ -151,36 +153,22 @@ "output_type": "stream", "text": [ "KarcherMeanFactorRot3: keys = { r0 r1 r2 }\n", + "sqrt(beta): 10.0\n", "\n", - "Linearized Factor (JacobianFactor):\n", - " A[r0] = [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " A[r1] = [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " A[r2] = [\n", - "\t1, 0, 0;\n", - "\t0, 1, 0;\n", - "\t0, 0, 1\n", - "]\n", - " b = [ 0 0 0 ]\n", - " No noise model\n" - ] - }, - { - "ename": "AttributeError", - "evalue": "'gtsam.gtsam.JacobianFactor' object has no attribute 'find'", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[9], line 20\u001b[0m\n\u001b[0;32m 18\u001b[0m sqrt_beta \u001b[38;5;241m=\u001b[39m np\u001b[38;5;241m.\u001b[39msqrt(beta)\n\u001b[0;32m 19\u001b[0m expected_jacobian \u001b[38;5;241m=\u001b[39m sqrt_beta \u001b[38;5;241m*\u001b[39m np\u001b[38;5;241m.\u001b[39meye(\u001b[38;5;241m3\u001b[39m)\n\u001b[1;32m---> 20\u001b[0m A0 \u001b[38;5;241m=\u001b[39m linearized_factor\u001b[38;5;241m.\u001b[39mgetA(\u001b[43mlinearized_factor\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mfind\u001b[49m(R(\u001b[38;5;241m0\u001b[39m)))\n\u001b[0;32m 21\u001b[0m A1 \u001b[38;5;241m=\u001b[39m linearized_factor\u001b[38;5;241m.\u001b[39mgetA(linearized_factor\u001b[38;5;241m.\u001b[39mfind(R(\u001b[38;5;241m1\u001b[39m)))\n\u001b[0;32m 22\u001b[0m A2 \u001b[38;5;241m=\u001b[39m linearized_factor\u001b[38;5;241m.\u001b[39mgetA(linearized_factor\u001b[38;5;241m.\u001b[39mfind(R(\u001b[38;5;241m2\u001b[39m)))\n", - "\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.JacobianFactor' object has no attribute 'find'" + "Jacobian for R(0):\n", + "[[10. 0. 0.]\n", + " [ 0. 10. 0.]\n", + " [ 0. 0. 10.]]\n", + "Jacobian for R(1):\n", + "[[10. 0. 0.]\n", + " [ 0. 10. 0.]\n", + " [ 0. 0. 10.]]\n", + "Jacobian for R(2):\n", + "[[10. 0. 0.]\n", + " [ 0. 10. 0.]\n", + " [ 0. 0. 10.]]\n", + "Error vector b:\n", + "[0. 0. 0.]\n" ] } ], @@ -188,7 +176,7 @@ "keys = [R(0), R(1), R(2)]\n", "beta = 100.0 # Strength of the constraint\n", "\n", - "k_factor = KarcherMeanFactorRot3(keys)\n", + "k_factor = KarcherMeanFactorRot3(keys, 3, beta)\n", "k_factor.print(\"KarcherMeanFactorRot3: \")\n", "\n", "# Linearization example\n", @@ -198,32 +186,27 @@ "values.insert(R(2), Rot3.Yaw(0.3))\n", "\n", "linearized_factor = k_factor.linearize(values)\n", - "print(\"\\nLinearized Factor (JacobianFactor):\")\n", - "linearized_factor.print()\n", "\n", "# Check the Jacobian blocks (should be sqrt(beta)*Identity)\n", "sqrt_beta = np.sqrt(beta)\n", - "expected_jacobian = sqrt_beta * np.eye(3)\n", - "A0 = linearized_factor.getA(linearized_factor.find(R(0)))\n", - "A1 = linearized_factor.getA(linearized_factor.find(R(1)))\n", - "A2 = linearized_factor.getA(linearized_factor.find(R(2)))\n", + "A = linearized_factor.getA()\n", + "assert A.shape == (3, 9), f\"Unexpected shape for A: {A.shape}\"\n", + "A0 = A[:, :3]\n", + "A1 = A[:, 3:6]\n", + "A2 = A[:, 6:9]\n", "b = linearized_factor.getb()\n", "\n", + "print(f\"sqrt(beta): {sqrt_beta}\")\n", "print(f\"\\nJacobian for R(0):\\n{A0}\")\n", "print(f\"Jacobian for R(1):\\n{A1}\")\n", "print(f\"Jacobian for R(2):\\n{A2}\")\n", - "print(f\"Error vector b:\\n{b}\")\n", - "print(f\"sqrt(beta): {sqrt_beta}\")\n", - "assert np.allclose(A0, expected_jacobian)\n", - "assert np.allclose(A1, expected_jacobian)\n", - "assert np.allclose(A2, expected_jacobian)\n", - "assert np.allclose(b, np.zeros(3))" + "print(f\"Error vector b:\\n{b}\")" ] } ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -237,7 +220,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, diff --git a/gtsam/slam/doc/PlanarProjectionFactor.ipynb b/gtsam/slam/doc/PlanarProjectionFactor.ipynb index 55d76252d..3100fbb53 100644 --- a/gtsam/slam/doc/PlanarProjectionFactor.ipynb +++ b/gtsam/slam/doc/PlanarProjectionFactor.ipynb @@ -15,9 +15,9 @@ "id": "desc_md" }, "source": [ - "The `PlanarProjectionFactor` variants provide camera projection constraints specifically designed for scenarios where the robot or camera moves primarily on a 2D plane (e.g., ground robots with cameras).\n", + "The `PlanarProjectionFactor` variants provide camera projection factors specifically designed for scenarios where **the robot or camera moves primarily on a 2D plane** (e.g., ground robots with cameras).\n", "They relate a 3D landmark point to a 2D pixel measurement observed by a camera, considering:\n", - "* The robot's 2D pose (`Pose2` `wTb`: world-to-body) in the ground plane.\n", + "* The robot's 2D pose (`Pose2` `wTb`: body in world frame) in the ground plane.\n", "* The camera's fixed 3D pose relative to the robot's body frame (`Pose3` `bTc`: body-to-camera).\n", "* The camera's intrinsic calibration (including distortion, typically `Cal3DS2` or similar).\n", "* The 3D landmark position in the world frame.\n", @@ -41,7 +41,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 1, "metadata": { "id": "pip_code", "tags": [ @@ -50,12 +50,16 @@ }, "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" ] }, { "cell_type": "code", - "execution_count": 1, + "execution_count": 2, "metadata": { "id": "imports_code" }, @@ -65,13 +69,7 @@ "import numpy as np\n", "from gtsam import (Pose2, Pose3, Point3, Point2, Rot3, Cal3DS2, Values,\n", " PlanarProjectionFactor1, PlanarProjectionFactor2, PlanarProjectionFactor3)\n", - "from gtsam import symbol_shorthand\n", - "import graphviz\n", - "\n", - "X = symbol_shorthand.X\n", - "L = symbol_shorthand.L\n", - "C = symbol_shorthand.C # Calibration\n", - "O = symbol_shorthand.O # Offset" + "from gtsam.symbol_shorthand import X, L, C, O" ] }, { @@ -110,19 +108,19 @@ "isotropic dim=2 sigma=1.5\n", "\n", "Error at ground truth: 0.0\n", - "Error at noisy pose: 3317.647263749095\n" + "Error at noisy pose: 3317.6472637491106\n" ] } ], "source": [ "# Known parameters\n", "landmark_pt = Point3(2.0, 0.5, 0.5)\n", - "body_T_cam = Pose3(Rot3.Yaw(-np.pi/2), Point3(0.1, 0, 0.2)) # Cam fwd = body +y\n", + "body_T_cam = Pose3(Rot3.Yaw(-np.pi / 2), Point3(0.1, 0, 0.2)) # Cam fwd = body +y\n", "calib = Cal3DS2(fx=500, fy=500, s=0, u0=320, v0=240, k1=0, k2=0, p1=0, p2=0)\n", - "measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.5) # Pixels\n", + "measurement_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.5) # Pixels\n", "\n", "# Assume ground truth pose and calculate expected measurement\n", - "gt_pose2 = Pose2(1.0, 0.0, np.pi/4)\n", + "gt_pose2 = Pose2(1.0, 0.0, np.pi / 4)\n", "gt_world_T_cam = Pose3(gt_pose2) * body_T_cam\n", "gt_camera = gtsam.PinholeCameraCal3DS2(gt_world_T_cam, calib)\n", "measured_pt2 = gt_camera.project(landmark_pt)\n", @@ -130,18 +128,19 @@ "print(f\"Calculated Measurement: {measured_pt2}\")\n", "\n", "# Create the factor\n", - "pose_key = X(0)\n", - "factor1 = PlanarProjectionFactor1(pose_key, landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise)\n", + "factor1 = PlanarProjectionFactor1(\n", + " X(0), landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise\n", + ")\n", "factor1.print(\"Factor 1: \")\n", "\n", "# Evaluate error\n", "values = Values()\n", - "values.insert(pose_key, gt_pose2) # Error should be zero here\n", + "values.insert(X(0), gt_pose2) # Error should be zero here\n", "error1_gt = factor1.error(values)\n", "print(f\"\\nError at ground truth: {error1_gt}\")\n", "\n", - "noisy_pose2 = Pose2(1.05, 0.02, np.pi/4 + 0.05)\n", - "values.update(pose_key, noisy_pose2)\n", + "noisy_pose2 = Pose2(1.05, 0.02, np.pi / 4 + 0.05)\n", + "values.update(X(0), noisy_pose2)\n", "error1_noisy = factor1.error(values)\n", "print(f\"Error at noisy pose: {error1_noisy}\")" ] @@ -175,37 +174,29 @@ "name": "stdout", "output_type": "stream", "text": [ - "Factor 2: keys = { l0 x0 }\n", - "isotropic dim=2 sigma=1.5\n" - ] - }, - { - "ename": "RuntimeError", - "evalue": "Attempting to retrieve value with key \"x0\", type stored in Values is class gtsam::GenericValue but requested type was class Eigen::Matrix", - "output_type": "error", - "traceback": [ - "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m", - "\u001b[1;31mRuntimeError\u001b[0m Traceback (most recent call last)", - "Cell \u001b[1;32mIn[4], line 10\u001b[0m\n\u001b[0;32m 8\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(pose_key, gt_pose2)\n\u001b[0;32m 9\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(landmark_key, landmark_pt) \u001b[38;5;66;03m# Error should be zero\u001b[39;00m\n\u001b[1;32m---> 10\u001b[0m error2_gt \u001b[38;5;241m=\u001b[39m \u001b[43mfactor2\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43merror\u001b[49m\u001b[43m(\u001b[49m\u001b[43mvalues\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 11\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mError at ground truth: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00merror2_gt\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 13\u001b[0m noisy_landmark \u001b[38;5;241m=\u001b[39m Point3(\u001b[38;5;241m2.1\u001b[39m, \u001b[38;5;241m0.45\u001b[39m, \u001b[38;5;241m0.55\u001b[39m)\n", - "\u001b[1;31mRuntimeError\u001b[0m: Attempting to retrieve value with key \"x0\", type stored in Values is class gtsam::GenericValue but requested type was class Eigen::Matrix" + "Factor 2: keys = { x0 l0 }\n", + "isotropic dim=2 sigma=1.5\n", + "\n", + "Error at ground truth: 0.0\n", + "Error with noisy landmark: 8066.192649473802\n" ] } ], "source": [ - "landmark_key = L(0)\n", - "\n", - "factor2 = PlanarProjectionFactor2(pose_key, landmark_key, measured_pt2, body_T_cam, calib, measurement_noise)\n", + "factor2 = PlanarProjectionFactor2(\n", + " X(0), L(0), measured_pt2, body_T_cam, calib, measurement_noise\n", + ")\n", "factor2.print(\"Factor 2: \")\n", "\n", "# Evaluate error\n", "values = Values()\n", - "values.insert(pose_key, gt_pose2)\n", - "values.insert(landmark_key, landmark_pt) # Error should be zero\n", + "values.insert(X(0), gt_pose2)\n", + "values.insert(L(0), landmark_pt) # Error should be zero\n", "error2_gt = factor2.error(values)\n", "print(f\"\\nError at ground truth: {error2_gt}\")\n", "\n", "noisy_landmark = Point3(2.1, 0.45, 0.55)\n", - "values.update(landmark_key, noisy_landmark)\n", + "values.update(L(0), noisy_landmark)\n", "error2_noisy = factor2.error(values)\n", "print(f\"Error with noisy landmark: {error2_noisy}\")" ] @@ -230,7 +221,7 @@ }, { "cell_type": "code", - "execution_count": 3, + "execution_count": 5, "metadata": { "id": "factor3_example_code" }, @@ -239,11 +230,11 @@ "name": "stdout", "output_type": "stream", "text": [ - "Factor 3: Factor NoiseModelFactor3 on x0 O0 C0\n", - "Noise model: diagonal sigmas [1.5; 1.5];\n", + "Factor 3: keys = { x0 o0 c0 }\n", + "isotropic dim=2 sigma=1.5\n", "\n", - "Error at ground truth: [-0. -0.]\n", - "Error with noisy calibration: [8.38867847 0.63659684]\n" + "Error at ground truth: 0.0\n", + "Error with noisy calibration: 92.30212176019934\n" ] } ], @@ -251,12 +242,12 @@ "offset_key = O(0)\n", "calib_key = C(0)\n", "\n", - "factor3 = PlanarProjectionFactor3(pose_key, offset_key, calib_key, landmark_pt, measured_pt2, measurement_noise)\n", + "factor3 = PlanarProjectionFactor3(X(0), offset_key, calib_key, landmark_pt, measured_pt2, measurement_noise)\n", "factor3.print(\"Factor 3: \")\n", "\n", "# Evaluate error\n", "values = Values()\n", - "values.insert(pose_key, gt_pose2)\n", + "values.insert(X(0), gt_pose2)\n", "values.insert(offset_key, body_T_cam)\n", "values.insert(calib_key, calib) # Error should be zero\n", "error3_gt = factor3.error(values)\n", @@ -271,7 +262,7 @@ ], "metadata": { "kernelspec": { - "display_name": "gtsam", + "display_name": "py312", "language": "python", "name": "python3" }, @@ -285,7 +276,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.13.1" + "version": "3.12.6" } }, "nbformat": 4, 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": "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", + "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 89c9943ce..911f4fd9a 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -562,9 +562,12 @@ template virtual class KarcherMeanFactor : gtsam::NonlinearFactor { KarcherMeanFactor(const gtsam::KeyVector& keys); + KarcherMeanFactor(const gtsam::KeyVector& keys, int d, double beta); }; -gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations); +template +T FindKarcherMean(const std::vector& elements); #include gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, @@ -638,6 +641,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()