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

Merge pull request #2116 from borglab/fix/new-docs-slam 

Fixed KarcherMean, LAGO, PlanarProjectionFactor
release/4.3a0
Porter Zach 2025-04-26 18:08:16 -04:00 committed by GitHub
parent e226cdde9f fa23a888f9
commit 9c1aa80d19
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
8 changed files with 221 additions and 190 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

2
gtsam/slam/PlanarProjectionFactor.h
View File

@ -201,7 +201,7 @@ namespace gtsam {
const Cal3DS2& calib, const Cal3DS2& calib,
const SharedNoiseModel& model = {}) const SharedNoiseModel& model = {})
: PlanarProjectionFactorBase(measured), : PlanarProjectionFactorBase(measured),
NoiseModelFactorN(model, landmarkKey, poseKey), NoiseModelFactorN(model, poseKey, landmarkKey),
bTc_(bTc), bTc_(bTc),
calib_(calib) {} calib_(calib) {}

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,

89
gtsam/slam/doc/PlanarProjectionFactor.ipynb
View File

@ -15,9 +15,9 @@
"id": "desc_md" "id": "desc_md"
}, },
"source": [ "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", "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 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 camera's intrinsic calibration (including distortion, typically `Cal3DS2` or similar).\n",
"* The 3D landmark position in the world frame.\n", "* The 3D landmark position in the world frame.\n",
@ -41,7 +41,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": null, "execution_count": 1,
"metadata": { "metadata": {
"id": "pip_code", "id": "pip_code",
"tags": [ "tags": [
@ -50,12 +50,16 @@
}, },
"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"
] ]
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 1, "execution_count": 2,
"metadata": { "metadata": {
"id": "imports_code" "id": "imports_code"
}, },
@ -65,13 +69,7 @@
"import numpy as np\n", "import numpy as np\n",
"from gtsam import (Pose2, Pose3, Point3, Point2, Rot3, Cal3DS2, Values,\n", "from gtsam import (Pose2, Pose3, Point3, Point2, Rot3, Cal3DS2, Values,\n",
" PlanarProjectionFactor1, PlanarProjectionFactor2, PlanarProjectionFactor3)\n", " PlanarProjectionFactor1, PlanarProjectionFactor2, PlanarProjectionFactor3)\n",
"from gtsam import symbol_shorthand\n", "from gtsam.symbol_shorthand import X, L, C, O"
"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"
] ]
}, },
{ {
@ -110,19 +108,19 @@
"isotropic dim=2 sigma=1.5\n", "isotropic dim=2 sigma=1.5\n",
"\n", "\n",
"Error at ground truth: 0.0\n", "Error at ground truth: 0.0\n",
"Error at noisy pose: 3317.647263749095\n" "Error at noisy pose: 3317.6472637491106\n"
] ]
} }
], ],
"source": [ "source": [
"# Known parameters\n", "# Known parameters\n",
"landmark_pt = Point3(2.0, 0.5, 0.5)\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", "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", "\n",
"# Assume ground truth pose and calculate expected measurement\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_world_T_cam = Pose3(gt_pose2) * body_T_cam\n",
"gt_camera = gtsam.PinholeCameraCal3DS2(gt_world_T_cam, calib)\n", "gt_camera = gtsam.PinholeCameraCal3DS2(gt_world_T_cam, calib)\n",
"measured_pt2 = gt_camera.project(landmark_pt)\n", "measured_pt2 = gt_camera.project(landmark_pt)\n",
@ -130,18 +128,19 @@
"print(f\"Calculated Measurement: {measured_pt2}\")\n", "print(f\"Calculated Measurement: {measured_pt2}\")\n",
"\n", "\n",
"# Create the factor\n", "# Create the factor\n",
"pose_key = X(0)\n", "factor1 = PlanarProjectionFactor1(\n",
"factor1 = PlanarProjectionFactor1(pose_key, landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise)\n", " X(0), landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise\n",
")\n",
"factor1.print(\"Factor 1: \")\n", "factor1.print(\"Factor 1: \")\n",
"\n", "\n",
"# Evaluate error\n", "# Evaluate error\n",
"values = Values()\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", "error1_gt = factor1.error(values)\n",
"print(f\"\\nError at ground truth: {error1_gt}\")\n", "print(f\"\\nError at ground truth: {error1_gt}\")\n",
"\n", "\n",
"noisy_pose2 = Pose2(1.05, 0.02, np.pi/4 + 0.05)\n", "noisy_pose2 = Pose2(1.05, 0.02, np.pi / 4 + 0.05)\n",
"values.update(pose_key, noisy_pose2)\n", "values.update(X(0), noisy_pose2)\n",
"error1_noisy = factor1.error(values)\n", "error1_noisy = factor1.error(values)\n",
"print(f\"Error at noisy pose: {error1_noisy}\")" "print(f\"Error at noisy pose: {error1_noisy}\")"
] ]
@ -175,37 +174,29 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"Factor 2: keys = { l0 x0 }\n", "Factor 2: keys = { x0 l0 }\n",
"isotropic dim=2 sigma=1.5\n" "isotropic dim=2 sigma=1.5\n",
] "\n",
}, "Error at ground truth: 0.0\n",
{ "Error with noisy landmark: 8066.192649473802\n"
"ename": "RuntimeError",
"evalue": "Attempting to retrieve value with key \"x0\", type stored in Values is class gtsam::GenericValue<class gtsam::Pose2> but requested type was class Eigen::Matrix<double,-1,1,0,-1,1>",
"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<class gtsam::Pose2> but requested type was class Eigen::Matrix<double,-1,1,0,-1,1>"
] ]
} }
], ],
"source": [ "source": [
"landmark_key = L(0)\n", "factor2 = PlanarProjectionFactor2(\n",
"\n", " X(0), L(0), measured_pt2, body_T_cam, calib, measurement_noise\n",
"factor2 = PlanarProjectionFactor2(pose_key, landmark_key, measured_pt2, body_T_cam, calib, measurement_noise)\n", ")\n",
"factor2.print(\"Factor 2: \")\n", "factor2.print(\"Factor 2: \")\n",
"\n", "\n",
"# Evaluate error\n", "# Evaluate error\n",
"values = Values()\n", "values = Values()\n",
"values.insert(pose_key, gt_pose2)\n", "values.insert(X(0), gt_pose2)\n",
"values.insert(landmark_key, landmark_pt) # Error should be zero\n", "values.insert(L(0), landmark_pt) # Error should be zero\n",
"error2_gt = factor2.error(values)\n", "error2_gt = factor2.error(values)\n",
"print(f\"\\nError at ground truth: {error2_gt}\")\n", "print(f\"\\nError at ground truth: {error2_gt}\")\n",
"\n", "\n",
"noisy_landmark = Point3(2.1, 0.45, 0.55)\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", "error2_noisy = factor2.error(values)\n",
"print(f\"Error with noisy landmark: {error2_noisy}\")" "print(f\"Error with noisy landmark: {error2_noisy}\")"
] ]
@ -230,7 +221,7 @@
}, },
{ {
"cell_type": "code", "cell_type": "code",
"execution_count": 3, "execution_count": 5,
"metadata": { "metadata": {
"id": "factor3_example_code" "id": "factor3_example_code"
}, },
@ -239,11 +230,11 @@
"name": "stdout", "name": "stdout",
"output_type": "stream", "output_type": "stream",
"text": [ "text": [
"Factor 3: Factor NoiseModelFactor3 on x0 O0 C0\n", "Factor 3: keys = { x0 o0 c0 }\n",
"Noise model: diagonal sigmas [1.5; 1.5];\n", "isotropic dim=2 sigma=1.5\n",
"\n", "\n",
"Error at ground truth: [-0. -0.]\n", "Error at ground truth: 0.0\n",
"Error with noisy calibration: [8.38867847 0.63659684]\n" "Error with noisy calibration: 92.30212176019934\n"
] ]
} }
], ],
@ -251,12 +242,12 @@
"offset_key = O(0)\n", "offset_key = O(0)\n",
"calib_key = C(0)\n", "calib_key = C(0)\n",
"\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", "factor3.print(\"Factor 3: \")\n",
"\n", "\n",
"# Evaluate error\n", "# Evaluate error\n",
"values = Values()\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(offset_key, body_T_cam)\n",
"values.insert(calib_key, calib) # Error should be zero\n", "values.insert(calib_key, calib) # Error should be zero\n",
"error3_gt = factor3.error(values)\n", "error3_gt = factor3.error(values)\n",
@ -271,7 +262,7 @@
], ],
"metadata": { "metadata": {
"kernelspec": { "kernelspec": {
"display_name": "gtsam", "display_name": "py312",
"language": "python", "language": "python",
"name": "python3" "name": "python3"
}, },
@ -285,7 +276,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,

183
gtsam/slam/doc/lago.ipynb
View File

File diff suppressed because one or more lines are too long

6
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,
@ -464,6 +467,7 @@ typedef gtsam::TriangulationFactor<gtsam::PinholePose<gtsam::Cal3Unified>>
namespace lago { namespace lago {
gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true); gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true);
gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess); gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess);
gtsam::VectorValues initializeOrientations(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true);
} }
} // namespace gtsam } // namespace gtsam

28
python/gtsam/tests/test_lago.py
View File

@ -13,7 +13,7 @@ import unittest
import numpy as np import numpy as np
import gtsam import gtsam
from gtsam import Point3, Pose2, PriorFactorPose2, Values from gtsam import BetweenFactorPose2, Point3, Pose2, PriorFactorPose2, Values
class TestLago(unittest.TestCase): class TestLago(unittest.TestCase):
@ -33,6 +33,32 @@ class TestLago(unittest.TestCase):
estimateLago: Values = gtsam.lago.initialize(graph) estimateLago: Values = gtsam.lago.initialize(graph)
assert isinstance(estimateLago, Values) 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__": if __name__ == "__main__":
unittest.main() unittest.main()