From 8fbe3c9c8cdd4e52133cee0fab40cefe81de3a48 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 26 Apr 2025 17:24:43 -0400 Subject: [PATCH] Fixed KarcherMean issue --- gtsam/linear/linear.i | 2 +- gtsam/slam/KarcherMeanFactor.h | 10 ++- gtsam/slam/doc/KarcherMeanFactor.ipynb | 91 +++++++++++--------------- gtsam/slam/slam.i | 5 +- 4 files changed, 49 insertions(+), 59 deletions(-) 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/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/slam.i b/gtsam/slam/slam.i index d8a2e49bf..50f2f0071 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -396,9 +396,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,