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

Merge branch 'new-docs-slam' of https://github.com/borglab/gtsam into new-docs-slam

release/4.3a0
p-zach 2025-04-26 20:17:58 -04:00
parent 57dbbfd9e3 9c1aa80d19
commit d245a7afde
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;
double error(const gtsam::VectorValues& c) const;
//Standard Interface
// Standard Interface
gtsam::Matrix getA() const;
gtsam::Vector getb() const;
size_t rows() const;

10
gtsam/slam/KarcherMeanFactor.h
View File

@ -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

2
gtsam/slam/PlanarProjectionFactor.h
View File

@ -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) {}

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

@ -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,

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

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

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

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

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