12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed KarcherMean issue

release/4.3a0
Frank Dellaert 2025-04-26 17:24:43 -04:00
parent e226cdde9f
commit 8fbe3c9c8c
4 changed files with 49 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/linear/linear.i
View File

@ -355,7 +355,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
gtsam::Vector error_vector(const gtsam::VectorValues& c) const; gtsam::Vector error_vector(const gtsam::VectorValues& c) const;
double error(const gtsam::VectorValues& c) const; double error(const gtsam::VectorValues& c) const;
//Standard Interface // Standard Interface
gtsam::Matrix getA() const; gtsam::Matrix getA() const;
gtsam::Vector getb() const; gtsam::Vector getb() const;
size_t rows() const; size_t rows() const;

10
gtsam/slam/KarcherMeanFactor.h
View File

@ -27,13 +27,17 @@
namespace gtsam { namespace gtsam {
/** /**
* Optimize for the Karcher mean, minimizing the geodesic distance to each of * 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. * PriorFactors.
*/ */
template <class T> template <class T>
T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &rotations); typename std::enable_if<traits<T>::IsLieGroup, T>::type
FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &elements);
template <class T> T FindKarcherMean(std::initializer_list<T> &&rotations); /// FindKarcherMean version from initializer list
template <class T>
typename std::enable_if<traits<T>::IsLieGroup, T>::type
FindKarcherMean(std::initializer_list<T> &&elements);
/** /**
* The KarcherMeanFactor creates a constraint on all SO(n) variables with * The KarcherMeanFactor creates a constraint on all SO(n) variables with

91
gtsam/slam/doc/KarcherMeanFactor.ipynb
View File

@ -15,12 +15,12 @@
"id": "desc_md" "id": "desc_md"
}, },
"source": [ "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", "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", "$$ \\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", "\n",
"Functions/Classes:\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<T>`: 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." "* `KarcherMeanFactor<T>`: 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", "cell_type": "code",
"execution_count": null, "execution_count": 1,
"metadata": { "metadata": {
"id": "pip_code", "id": "pip_code",
"tags": [ "tags": [
@ -44,7 +44,11 @@
}, },
"outputs": [], "outputs": [],
"source": [ "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": [ "source": [
"import gtsam\n", "import gtsam\n",
"import numpy as np\n", "import numpy as np\n",
"from gtsam import Rot3, FindKarcherMean, KarcherMeanFactorRot3, NonlinearFactorGraph, Values\n", "from gtsam import Rot3, FindKarcherMeanRot3, KarcherMeanFactorRot3, Values\n",
"from gtsam import symbol_shorthand\n", "from gtsam.symbol_shorthand import R"
"\n",
"R = symbol_shorthand.R"
] ]
}, },
{ {
@ -111,7 +113,7 @@
"rotations.append(Rot3.Yaw(0.12))\n", "rotations.append(Rot3.Yaw(0.12))\n",
"\n", "\n",
"# Compute the Karcher mean\n", "# Compute the Karcher mean\n",
"karcher_mean = FindKarcherMean(rotations)\n", "karcher_mean = FindKarcherMeanRot3(rotations)\n",
"\n", "\n",
"print(\"Input Rotations (Yaw angles):\")\n", "print(\"Input Rotations (Yaw angles):\")\n",
"for r in rotations: print(f\" {r.yaw():.3f}\")\n", "for r in rotations: print(f\" {r.yaw():.3f}\")\n",
@ -141,7 +143,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 9, "execution_count": 6,
"metadata": { "metadata": {
"id": "factor_example_code" "id": "factor_example_code"
}, },
@ -151,36 +153,22 @@
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"KarcherMeanFactorRot3: keys = { r0 r1 r2 }\n", "KarcherMeanFactorRot3: keys = { r0 r1 r2 }\n",
"sqrt(beta): 10.0\n",
"\n", "\n",
"Linearized Factor (JacobianFactor):\n", "Jacobian for R(0):\n",
" A[r0] = [\n", "[[10. 0. 0.]\n",
"\t1, 0, 0;\n", " [ 0. 10. 0.]\n",
"\t0, 1, 0;\n", " [ 0. 0. 10.]]\n",
"\t0, 0, 1\n", "Jacobian for R(1):\n",
"]\n", "[[10. 0. 0.]\n",
" A[r1] = [\n", " [ 0. 10. 0.]\n",
"\t1, 0, 0;\n", " [ 0. 0. 10.]]\n",
"\t0, 1, 0;\n", "Jacobian for R(2):\n",
"\t0, 0, 1\n", "[[10. 0. 0.]\n",
"]\n", " [ 0. 10. 0.]\n",
" A[r2] = [\n", " [ 0. 0. 10.]]\n",
"\t1, 0, 0;\n", "Error vector b:\n",
"\t0, 1, 0;\n", "[0. 0. 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'"
] ]
} }
], ],
@ -188,7 +176,7 @@
"keys = [R(0), R(1), R(2)]\n", "keys = [R(0), R(1), R(2)]\n",
"beta = 100.0 # Strength of the constraint\n", "beta = 100.0 # Strength of the constraint\n",
"\n", "\n",
"k_factor = KarcherMeanFactorRot3(keys)\n", "k_factor = KarcherMeanFactorRot3(keys, 3, beta)\n",
"k_factor.print(\"KarcherMeanFactorRot3: \")\n", "k_factor.print(\"KarcherMeanFactorRot3: \")\n",
"\n", "\n",
"# Linearization example\n", "# Linearization example\n",
@ -198,32 +186,27 @@
"values.insert(R(2), Rot3.Yaw(0.3))\n", "values.insert(R(2), Rot3.Yaw(0.3))\n",
"\n", "\n",
"linearized_factor = k_factor.linearize(values)\n", "linearized_factor = k_factor.linearize(values)\n",
"print(\"\\nLinearized Factor (JacobianFactor):\")\n",
"linearized_factor.print()\n",
"\n", "\n",
"# Check the Jacobian blocks (should be sqrt(beta)*Identity)\n", "# Check the Jacobian blocks (should be sqrt(beta)*Identity)\n",
"sqrt_beta = np.sqrt(beta)\n", "sqrt_beta = np.sqrt(beta)\n",
"expected_jacobian = sqrt_beta * np.eye(3)\n", "A = linearized_factor.getA()\n",
"A0 = linearized_factor.getA(linearized_factor.find(R(0)))\n", "assert A.shape == (3, 9), f\"Unexpected shape for A: {A.shape}\"\n",
"A1 = linearized_factor.getA(linearized_factor.find(R(1)))\n", "A0 = A[:, :3]\n",
"A2 = linearized_factor.getA(linearized_factor.find(R(2)))\n", "A1 = A[:, 3:6]\n",
"A2 = A[:, 6:9]\n",
"b = linearized_factor.getb()\n", "b = linearized_factor.getb()\n",
"\n", "\n",
"print(f\"sqrt(beta): {sqrt_beta}\")\n",
"print(f\"\\nJacobian for R(0):\\n{A0}\")\n", "print(f\"\\nJacobian for R(0):\\n{A0}\")\n",
"print(f\"Jacobian for R(1):\\n{A1}\")\n", "print(f\"Jacobian for R(1):\\n{A1}\")\n",
"print(f\"Jacobian for R(2):\\n{A2}\")\n", "print(f\"Jacobian for R(2):\\n{A2}\")\n",
"print(f\"Error vector b:\\n{b}\")\n", "print(f\"Error vector b:\\n{b}\")"
"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))"
] ]
} }
], ],
"metadata": { "metadata": {
"kernelspec": { "kernelspec": {
"display_name": "gtsam", "display_name": "py312",
"language": "python", "language": "python",
"name": "python3" "name": "python3"
}, },
@ -237,7 +220,7 @@
"name": "python", "name": "python",
"nbconvert_exporter": "python", "nbconvert_exporter": "python",
"pygments_lexer": "ipython3", "pygments_lexer": "ipython3",
"version": "3.13.1" "version": "3.12.6"
} }
}, },
"nbformat": 4, "nbformat": 4,

5
gtsam/slam/slam.i
View File

@ -396,9 +396,12 @@ template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}> gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
virtual class KarcherMeanFactor : gtsam::NonlinearFactor { virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
KarcherMeanFactor(const gtsam::KeyVector& keys); KarcherMeanFactor(const gtsam::KeyVector& keys);
KarcherMeanFactor(const gtsam::KeyVector& keys, int d, double beta);
}; };
gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations); template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
T FindKarcherMean(const std::vector<T>& elements);
#include <gtsam/slam/FrobeniusFactor.h> #include <gtsam/slam/FrobeniusFactor.h>
gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model, gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,