Fixed KarcherMean issue
Frank Dellaert
parent
e226cdde9f
commit
8fbe3c9c8c
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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."
|
||||
]
|
||||
},
|
||||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -396,9 +396,12 @@ template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
|
|||
gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
|
||||
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 = {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>
|
||||
gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,
|
||||
|
|
|
|||
Loading…
Reference in New Issue