Merge pull request #2116 from borglab/fix/new-docs-slam
Fixed KarcherMean, LAGO, PlanarProjectionFactorrelease/4.3a0
Porter Zach
GitHub
commit
9c1aa80d19
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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) {}
|
||||
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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",
|
||||
"\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,
|
||||
|
|
|
|||
File diff suppressed because one or more lines are too long
|
|
@ -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,
|
||||
|
|
@ -464,6 +467,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
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
Loading…
Reference in New Issue