Skip to content

Fixed KarcherMean, LAGO, PlanarProjectionFactor #2116

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
p-zach merged 3 commits into from
Apr 26, 2025
Merged

Fixed KarcherMean, LAGO, PlanarProjectionFactor #2116

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
8fbe3c9
Fixed KarcherMean issue
dellaert Apr 26, 2025
3e13c44
Fix LAGO docs, add test
dellaert Apr 26, 2025
fa23a88
Fixed PlanarProjectionFactor
dellaert Apr 26, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion gtsam/linear/linear.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
10 changes: 7 additions & 3 deletions gtsam/slam/KarcherMeanFactor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 <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
Expand Down
2 changes: 1 addition & 1 deletion gtsam/slam/PlanarProjectionFactor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {}

Expand Down
91 changes: 37 additions & 54 deletions gtsam/slam/doc/KarcherMeanFactor.ipynb
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<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."
]
},
Expand All @@ -35,7 +35,7 @@
},
{
"cell_type": "code",
"execution_count": null,
"execution_count": 1,
"metadata": {
"id": "pip_code",
"tags": [
Expand All @@ -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"
]
},
{
Expand All @@ -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"
]
},
{
Expand Down Expand Up @@ -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",
Expand Down Expand Up @@ -141,7 +143,7 @@
},
{
"cell_type": "code",
"execution_count": 9,
"execution_count": 6,
"metadata": {
"id": "factor_example_code"
},
Expand All @@ -151,44 +153,30 @@
"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"
]
}
],
"source": [
"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",
Expand All @@ -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"
},
Expand All @@ -237,7 +220,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
"version": "3.12.6"
}
},
"nbformat": 4,
Expand Down
Loading
Loading