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

New SLAM notebooks

release/4.3a0
p-zach 2025-04-24 10:52:25 -04:00
parent 3eac859bf9
commit c28c9b58d6
22 changed files with 5916 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

322
gtsam/slam/doc/BetweenFactor.ipynb Normal file
View File

@ -0,0 +1,322 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# BetweenFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`BetweenFactor<VALUE>` is a fundamental factor in GTSAM used to represent measurements of the relative transformation (motion) between two variables of the same type `VALUE`.\n",
"Common examples include:\n",
"* `BetweenFactorPose2`: Represents odometry measurements between 2D robot poses.\n",
"* `BetweenFactorPose3`: Represents odometry or relative pose estimates between 3D poses.\n",
"\n",
"The `VALUE` type must be a Lie Group (e.g., `Pose2`, `Pose3`, `Rot2`, `Rot3`).\n",
"\n",
"The factor encodes a constraint based on the measurement `measured_`, such that the error is calculated based on the difference between the predicted relative transformation and the measured one. Specifically, if the factor connects keys $k_1$ and $k_2$ corresponding to values $X_1$ and $X_2$, and the measurement is $Z$, the factor aims to minimize:\n",
"\n",
"$$ \\text{error}(X_1, X_2) = \\text{Log}(Z^{-1} \\cdot (X_1^{-1} \\cdot X_2)) $$\n",
"\n",
"where `Log` is the logarithmic map for the Lie group `VALUE`, $X_1^{-1} \\cdot X_2$ is the predicted relative transformation `between(X1, X2)`, and $Z^{-1}$ is the inverse of the measurement. The error vector lives in the tangent space of the identity element of the group.\n",
"\n",
"`BetweenConstraint<VALUE>` is a derived class that uses a `noiseModel::Constrained` noise model, effectively enforcing the relative transformation to be exactly the measured value."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/BetweenFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import BetweenFactorPose2, BetweenFactorPose3\n",
"from gtsam import Pose2, Pose3, Rot3, Point3\n",
"from gtsam import NonlinearFactorGraph, Values\n",
"from gtsam import symbol_shorthand\n",
"import graphviz\n",
"\n",
"X = symbol_shorthand.X"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_header_md"
},
"source": [
"## Creating a BetweenFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_desc_md"
},
"source": [
"You create a `BetweenFactor` by specifying:\n",
"1. The keys of the two variables it connects (e.g., `X(0)`, `X(1)`).\n",
"2. The measured relative transformation (e.g., a `Pose2` or `Pose3`).\n",
"3. A noise model describing the uncertainty of the measurement."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"BetweenFactorPose2: BetweenFactor(x0,x1)\n",
" measured: (1, 0, 0)\n",
" noise model: diagonal sigmas [0.2; 0.2; 0.1];\n",
"\n",
"BetweenFactorPose3: BetweenFactor(x1,x2)\n",
" measured: R: [\n",
"\t0.995004165, -0.0998334166, 0;\n",
"\t0.0998334166, 0.995004165, 0;\n",
"\t0, 0, 1\n",
"]\n",
"t: 0.5 0 0\n",
" noise model: diagonal sigmas [0.05; 0.05; 0.05; 0.1; 0.1; 0.1];\n"
]
}
],
"source": [
"# Example for Pose2 (2D SLAM odometry)\n",
"key1 = X(0)\n",
"key2 = X(1)\n",
"measured_pose2 = Pose2(1.0, 0.0, 0.0) # Move 1 meter forward\n",
"odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n",
"\n",
"between_factor_pose2 = BetweenFactorPose2(key1, key2, measured_pose2, odometry_noise)\n",
"between_factor_pose2.print(\"BetweenFactorPose2: \")\n",
"\n",
"# Example for Pose3 (3D SLAM odometry)\n",
"measured_pose3 = Pose3(Rot3.Yaw(0.1), Point3(0.5, 0, 0)) # Move 0.5m forward, yaw 0.1 rad\n",
"odometry_noise_3d = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05, 0.05, 0.05, 0.1, 0.1, 0.1]))\n",
"\n",
"between_factor_pose3 = BetweenFactorPose3(X(1), X(2), measured_pose3, odometry_noise_3d)\n",
"between_factor_pose3.print(\"\\nBetweenFactorPose3: \")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_header_md"
},
"source": [
"## Evaluating the Error"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_desc_md"
},
"source": [
"The `.error(values)` method calculates the error vector based on the current estimates of the variables in the `Values` object."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Error vector for BetweenFactorPose2: 0.3750000000000002\n",
"Manual unwhitened error calculation: [0.10247917 0.09747917 0.05 ]\n",
"Factor unwhitened error: [0.1 0.1 0.05]\n",
"Manually whitened error: [0.51239583 0.48739583 0.5 ]\n"
]
}
],
"source": [
"values = Values()\n",
"values.insert(X(0), Pose2(0.0, 0.0, 0.0))\n",
"values.insert(X(1), Pose2(1.1, 0.1, 0.05)) # Slightly off from measurement\n",
"\n",
"# Evaluate the error for the Pose2 factor\n",
"error_vector = between_factor_pose2.error(values)\n",
"print(f\"Error vector for BetweenFactorPose2: {error_vector}\")\n",
"\n",
"# The unwhitened error is Log(Z^-1 * (X1^-1 * X2))\n",
"pose0 = values.atPose2(X(0))\n",
"pose1 = values.atPose2(X(1))\n",
"predicted_relative = pose0.between(pose1)\n",
"error_pose = measured_pose2.inverse() * predicted_relative\n",
"unwhitened_error_expected = Pose2.Logmap(error_pose)\n",
"print(f\"Manual unwhitened error calculation: {unwhitened_error_expected}\")\n",
"print(f\"Factor unwhitened error: {between_factor_pose2.unwhitenedError(values)}\")\n",
"\n",
"# The whitened error (returned by error()) applies the noise model\n",
"# For diagonal noise model, error_vector = unwhitened_error / sigmas\n",
"sigmas = odometry_noise.sigmas()\n",
"whitened_expected = unwhitened_error_expected / sigmas\n",
"print(f\"Manually whitened error: {whitened_expected}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "viz_header_md"
},
"source": [
"## Visualization"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {
"id": "viz_example_code"
},
"outputs": [
{
"data": {
"image/svg+xml": [
"<?xml version=\"1.0\" encoding=\"UTF-8\" standalone=\"no\"?>\n",
"<!DOCTYPE svg PUBLIC \"-//W3C//DTD SVG 1.1//EN\"\n",
" \"http://www.w3.org/Graphics/SVG/1.1/DTD/svg11.dtd\">\n",
"<!-- Generated by graphviz version 2.50.0 (0)\n",
" -->\n",
"<!-- Pages: 1 -->\n",
"<svg width=\"206pt\" height=\"84pt\"\n",
" viewBox=\"0.00 0.00 206.00 83.60\" xmlns=\"http://www.w3.org/2000/svg\" xmlns:xlink=\"http://www.w3.org/1999/xlink\">\n",
"<g id=\"graph0\" class=\"graph\" transform=\"scale(1 1) rotate(0) translate(4 79.6)\">\n",
"<polygon fill=\"white\" stroke=\"transparent\" points=\"-4,4 -4,-79.6 202,-79.6 202,4 -4,4\"/>\n",
"<!-- var8646911284551352320 -->\n",
"<g id=\"node1\" class=\"node\">\n",
"<title>var8646911284551352320</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"27\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"27\" y=\"-53.9\" font-family=\"Times New Roman,serif\" font-size=\"14.00\">x0</text>\n",
"</g>\n",
"<!-- factor0 -->\n",
"<g id=\"node4\" class=\"node\">\n",
"<title>factor0</title>\n",
"<ellipse fill=\"black\" stroke=\"black\" cx=\"75\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
"</g>\n",
"<!-- var8646911284551352320&#45;&#45;factor0 -->\n",
"<g id=\"edge1\" class=\"edge\">\n",
"<title>var8646911284551352320&#45;&#45;factor0</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M40.37,-41.61C52.8,-27.68 69.99,-8.41 74.09,-3.81\"/>\n",
"</g>\n",
"<!-- var8646911284551352321 -->\n",
"<g id=\"node2\" class=\"node\">\n",
"<title>var8646911284551352321</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"99\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"99\" y=\"-53.9\" font-family=\"Times New Roman,serif\" font-size=\"14.00\">x1</text>\n",
"</g>\n",
"<!-- var8646911284551352321&#45;&#45;factor0 -->\n",
"<g id=\"edge2\" class=\"edge\">\n",
"<title>var8646911284551352321&#45;&#45;factor0</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M91.67,-40.17C85.49,-26.32 77.36,-8.1 75.43,-3.76\"/>\n",
"</g>\n",
"<!-- factor1 -->\n",
"<g id=\"node5\" class=\"node\">\n",
"<title>factor1</title>\n",
"<ellipse fill=\"black\" stroke=\"black\" cx=\"122\" cy=\"-1.8\" rx=\"1.8\" ry=\"1.8\"/>\n",
"</g>\n",
"<!-- var8646911284551352321&#45;&#45;factor1 -->\n",
"<g id=\"edge3\" class=\"edge\">\n",
"<title>var8646911284551352321&#45;&#45;factor1</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M106.03,-40.17C111.94,-26.32 119.74,-8.1 121.59,-3.76\"/>\n",
"</g>\n",
"<!-- var8646911284551352322 -->\n",
"<g id=\"node3\" class=\"node\">\n",
"<title>var8646911284551352322</title>\n",
"<ellipse fill=\"none\" stroke=\"black\" cx=\"171\" cy=\"-57.6\" rx=\"27\" ry=\"18\"/>\n",
"<text text-anchor=\"middle\" x=\"171\" y=\"-53.9\" font-family=\"Times New Roman,serif\" font-size=\"14.00\">x2</text>\n",
"</g>\n",
"<!-- var8646911284551352322&#45;&#45;factor1 -->\n",
"<g id=\"edge4\" class=\"edge\">\n",
"<title>var8646911284551352322&#45;&#45;factor1</title>\n",
"<path fill=\"none\" stroke=\"black\" d=\"M157.61,-41.9C144.92,-27.96 127.17,-8.48 122.94,-3.83\"/>\n",
"</g>\n",
"</g>\n",
"</svg>\n"
],
"text/plain": [
"<graphviz.sources.Source at 0x2622f1f9fd0>"
]
},
"execution_count": 5,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"graph = NonlinearFactorGraph()\n",
"graph.add(between_factor_pose2)\n",
"graph.add(between_factor_pose3)\n",
"\n",
"dot_string = graph.dot(values)\n",
"graphviz.Source(dot_string)"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

212
gtsam/slam/doc/EssentialMatrixConstraint.ipynb Normal file
View File

@ -0,0 +1,212 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# EssentialMatrixConstraint"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`EssentialMatrixConstraint` is a binary factor connecting two `Pose3` variables.\n",
"It represents a constraint derived from a measured Essential matrix ($E$) between the two camera views corresponding to the poses.\n",
"The Essential matrix $E$ encapsulates the relative rotation $R$ and translation direction $t$ between two *calibrated* cameras according to the epipolar constraint:\n",
"$$ p_2^T E p_1 = 0 $$\n",
"where $p_1$ and $p_2$ are the homogeneous coordinates of corresponding points in the *normalized (calibrated)* image planes.\n",
"\n",
"This factor takes the measured $E_{12}$ (from pose 1 to pose 2) and compares it to the Essential matrix predicted from the estimated poses $P_1$ and $P_2$.\n",
"The error is computed in the 5-dimensional tangent space of the Essential matrix manifold.\n",
"\n",
"**Note:** This factor requires known camera calibration, as the Essential matrix operates on normalized image coordinates."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/EssentialMatrixConstraint.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import EssentialMatrixConstraint, EssentialMatrix, Pose3, Rot3, Point3, Values\n",
"from gtsam import symbol_shorthand\n",
"\n",
"X = symbol_shorthand.X"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_header_md"
},
"source": [
"## Creating an EssentialMatrixConstraint"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_desc_md"
},
"source": [
"To create the factor, provide:\n",
"1. Keys for the two `Pose3` variables (e.g., `X(1)`, `X(2)`).\n",
"2. The measured `gtsam.EssentialMatrix` ($E_{12}$).\n",
"3. A 5-dimensional noise model (`gtsam.noiseModel`)."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"EssentialMatrixConstraint: EssentialMatrixConstraint(x1,x2)\n",
" measured: R:\n",
" [\n",
"\t0.995004, -0.0998334, 0;\n",
"\t0.0998334, 0.995004, 0;\n",
"\t0, 0, 1\n",
"]\n",
"d: :1\n",
"0\n",
"0\n",
"isotropic dim=5 sigma=0.01\n"
]
}
],
"source": [
"# Assume we have two poses\n",
"pose1 = Pose3(Rot3.Yaw(0.0), Point3(0, 0, 0))\n",
"pose2 = Pose3(Rot3.Yaw(0.1), Point3(1, 0, 0))\n",
"\n",
"# Calculate the ground truth Essential matrix between them\n",
"gt_E12 = EssentialMatrix.FromPose3(pose1.between(pose2))\n",
"\n",
"# Add some noise (conceptual, E lives on a manifold)\n",
"# In practice, E would be estimated from image correspondences\n",
"measured_E = gt_E12 # Use ground truth for this example\n",
"\n",
"# Define a noise model (5 dimensional!)\n",
"noise_dim = 5\n",
"E_noise = gtsam.noiseModel.Isotropic.Sigma(noise_dim, 0.01)\n",
"\n",
"# Create the factor\n",
"key1 = X(1)\n",
"key2 = X(2)\n",
"factor = EssentialMatrixConstraint(key1, key2, measured_E, E_noise)\n",
"\n",
"factor.print(\"EssentialMatrixConstraint: \")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_header_md"
},
"source": [
"## Evaluating the Error"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_desc_md"
},
"source": [
"The `.error(values)` method computes the error vector in the 5D tangent space of the Essential matrix manifold. The error represents the difference between the measured E and the E predicted from the current pose estimates in `values`."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Error vector at ground truth: 0.0\n",
"Error vector at noisy pose: 1.4069198000486227\n"
]
}
],
"source": [
"values = Values()\n",
"# Insert values close to ground truth\n",
"values.insert(key1, pose1)\n",
"values.insert(key2, pose2)\n",
"\n",
"error_vector = factor.error(values)\n",
"print(f\"Error vector at ground truth: {error_vector}\")\n",
"\n",
"# Insert values slightly different\n",
"noisy_pose2 = Pose3(Rot3.Yaw(0.11), Point3(1.05, 0.01, -0.01))\n",
"values.update(key2, noisy_pose2)\n",
"\n",
"error_vector_noisy = factor.error(values)\n",
"print(f\"Error vector at noisy pose: {error_vector_noisy}\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

392
gtsam/slam/doc/EssentialMatrixFactor.ipynb Normal file
View File

@ -0,0 +1,392 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# EssentialMatrixFactor Variants"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"This header defines several factors related to the Essential matrix ($E$), which encodes the relative rotation and translation direction between two *calibrated* cameras.\n",
"They are primarily used in Structure from Motion (SfM) problems where point correspondences between views are available but the 3D structure and/or camera poses/calibration are unknown.\n",
"\n",
"The core constraint is the epipolar constraint: $p_2^T E p_1 = 0$, where $p_1$ and $p_2$ are corresponding points in *normalized (calibrated)* image coordinates.\n",
"\n",
"Factors defined here:\n",
"* `EssentialMatrixFactor`: Constrains an unknown `EssentialMatrix` variable using a single point correspondence $(p_1, p_2)$.\n",
"* `EssentialMatrixFactor2`: Constrains an `EssentialMatrix` and an unknown inverse depth variable using a point correspondence.\n",
"* `EssentialMatrixFactor3`: Like Factor2, but incorporates an additional fixed extrinsic rotation (useful for camera rigs).\n",
"* `EssentialMatrixFactor4<CALIBRATION>`: Constrains an `EssentialMatrix` and a *shared* `CALIBRATION` variable using a point correspondence given in *pixel* coordinates.\n",
"* `EssentialMatrixFactor5<CALIBRATION>`: Constrains an `EssentialMatrix` and *two* unknown `CALIBRATION` variables (one for each camera) using a point correspondence given in *pixel* coordinates."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/EssentialMatrixFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import (EssentialMatrix, Point2, Point3, Rot3, Unit3, Pose3, Cal3_S2, EssentialMatrixFactor,\n",
" EssentialMatrixFactor2, EssentialMatrixFactor3, EssentialMatrixFactor4Cal3_S2,\n",
" EssentialMatrixFactor5Cal3_S2, Values)\n",
"from gtsam import symbol_shorthand\n",
"\n",
"E = symbol_shorthand.E\n",
"K = symbol_shorthand.K\n",
"D = symbol_shorthand.D # For inverse depth"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor1_header_md"
},
"source": [
"## 1. `EssentialMatrixFactor`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor1_desc_md"
},
"source": [
"This factor involves a single `EssentialMatrix` variable. It takes a pair of corresponding points $(p_A, p_B)$ in *normalized (calibrated)* image coordinates and penalizes deviations from the epipolar constraint $p_B^T E p_A = 0$.\n",
"The error is $p_B^T E p_A$."
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {
"id": "factor1_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Factor 1: keys = { e0 }\n",
"isotropic dim=1 sigma=0.01\n",
" EssentialMatrixFactor with measurements\n",
" (0.5 0.2 1)' and ( 0.4 0.25 1)'\n",
"\n",
"Error for Factor 1: 12.499999999999995\n"
]
}
],
"source": [
"# Assume normalized coordinates\n",
"pA_calibrated = Point2(0.5, 0.2)\n",
"pB_calibrated = Point2(0.4, 0.25)\n",
"\n",
"# Noise model on the epipolar error (scalar)\n",
"epipolar_noise = gtsam.noiseModel.Isotropic.Sigma(1, 0.01)\n",
"\n",
"# Key for the unknown Essential Matrix\n",
"keyE = E(0)\n",
"\n",
"factor1 = EssentialMatrixFactor(keyE, pA_calibrated, pB_calibrated, epipolar_noise)\n",
"factor1.print(\"Factor 1: \")\n",
"\n",
"# Evaluate error (requires an EssentialMatrix value)\n",
"values = Values()\n",
"# Example: E for identity rotation, translation (1,0,0)\n",
"example_E = EssentialMatrix(Rot3(), Unit3(1, 0, 0))\n",
"values.insert(keyE, example_E)\n",
"error1 = factor1.error(values)\n",
"print(f\"\\nError for Factor 1: {error1}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor2_header_md"
},
"source": [
"## 2. `EssentialMatrixFactor2`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor2_desc_md"
},
"source": [
"This factor involves an `EssentialMatrix` variable and a `double` variable representing the *inverse depth* of the 3D point corresponding to the measurement $p_A$ in the first camera's frame.\n",
"It assumes the measurement $p_B$ is perfect and calculates the reprojection error of the point (reconstructed using $p_A$ and the inverse depth) in the first camera image, after projecting it into the second camera and back.\n",
"It requires point correspondences $(p_A, p_B)$, which can be provided in either calibrated or pixel coordinates (if a calibration object `K` is provided).\n",
"The error is a 2D reprojection error in the first image plane (typically in pixels if K is provided)."
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {
"id": "factor2_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Factor 2: keys = { e0 d0 }\n",
" noise model: unit (2) \n",
" EssentialMatrixFactor2 with measurements\n",
" (480 288 1)' and (464 312)'\n",
"\n",
"Error for Factor 2: 412.82000000000016\n"
]
}
],
"source": [
"# Assume pixel coordinates and known calibration\n",
"K_cal = Cal3_S2(500, 500, 0, 320, 240)\n",
"pA_pixels = Point2(480, 288) # Corresponds to (0.5, 0.2) calibrated\n",
"pB_pixels = Point2(464, 312) # Corresponds to (0.4, 0.25) calibrated\n",
"\n",
"# Noise model on the 2D reprojection error (pixels)\n",
"reprojection_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
"\n",
"# Key for inverse depth\n",
"keyD = D(0)\n",
"\n",
"factor2 = EssentialMatrixFactor2(keyE, keyD, pA_pixels, pB_pixels, reprojection_noise)\n",
"factor2.print(\"\\nFactor 2: \")\n",
"\n",
"# Evaluate error (requires E and inverse depth d)\n",
"values.insert(keyD, 0.2) # Assume inverse depth d = 1/Z = 1/5 = 0.2\n",
"error2 = factor2.error(values)\n",
"print(f\"\\nError for Factor 2: {error2}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor3_header_md"
},
"source": [
"## 3. `EssentialMatrixFactor3`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor3_desc_md"
},
"source": [
"This is identical to `EssentialMatrixFactor2` but includes an additional fixed `Rot3` representing the rotation from the 'body' frame (where the Essential matrix is defined) to the 'camera' frame (where the measurements are made).\n",
"`iRc`: Rotation from camera frame to body frame (inverse of body-to-camera).\n",
"The Essential matrix $E_{body}$ is transformed to the camera frame before use: $E_{camera} = R_{cRb} \\cdot E_{body}$."
]
},
{
"cell_type": "code",
"execution_count": 11,
"metadata": {
"id": "factor3_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Factor 3: keys = { e0 d0 }\n",
" noise model: unit (2) \n",
" EssentialMatrixFactor2 with measurements\n",
" (480 288 1)' and (464 312)'\n",
" EssentialMatrixFactor3 with rotation [\n",
"\t0.99875, -0.0499792, 0;\n",
"\t0.0499792, 0.99875, 0;\n",
"\t0, 0, 1\n",
"]\n",
"\n",
"Error for Factor 3: 413.0638991792357\n"
]
}
],
"source": [
"body_R_cam = Rot3.Yaw(0.05) # Example fixed rotation\n",
"\n",
"factor3 = EssentialMatrixFactor3(keyE, keyD, pA_pixels, pB_pixels, body_R_cam, reprojection_noise)\n",
"factor3.print(\"\\nFactor 3: \")\n",
"\n",
"# Evaluate error (uses same E and d from values)\n",
"error3 = factor3.error(values)\n",
"print(f\"\\nError for Factor 3: {error3}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor4_header_md"
},
"source": [
"## 4. `EssentialMatrixFactor4<CALIBRATION>`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor4_desc_md"
},
"source": [
"This factor involves an `EssentialMatrix` variable and a single unknown `CALIBRATION` variable (e.g., `Cal3_S2`) that is assumed to be **shared** by both cameras.\n",
"It takes point correspondences $(p_A, p_B)$ in *pixel* coordinates.\n",
"The error is the algebraic epipolar error $ (K^{-1} p_B)^T E (K^{-1} p_A) $.\n",
"\n",
"**Note:** Recovering calibration from 2D correspondences alone is often ill-posed. This factor typically requires strong priors on the calibration."
]
},
{
"cell_type": "code",
"execution_count": 12,
"metadata": {
"id": "factor4_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Factor 4: keys = { e0 k0 }\n",
"isotropic dim=1 sigma=0.01\n",
" EssentialMatrixFactor4 with measurements\n",
" (480 288)' and (464 312)'\n",
"\n",
"Error for Factor 4: 11.520000000000007\n"
]
}
],
"source": [
"# Key for the unknown shared Calibration\n",
"keyK = K(0)\n",
"\n",
"factor4 = EssentialMatrixFactor4Cal3_S2(keyE, keyK, pA_pixels, pB_pixels, epipolar_noise)\n",
"factor4.print(\"\\nFactor 4: \")\n",
"\n",
"# Evaluate error (requires E and K)\n",
"values.insert(keyK, K_cal) # Use the known K for this example\n",
"error4 = factor4.error(values)\n",
"print(f\"\\nError for Factor 4: {error4}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor5_header_md"
},
"source": [
"## 5. `EssentialMatrixFactor5<CALIBRATION>`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor5_desc_md"
},
"source": [
"Similar to Factor4, but allows for **two different** unknown `CALIBRATION` variables, one for each camera ($K_A$ and $K_B$).\n",
"It takes point correspondences $(p_A, p_B)$ in *pixel* coordinates.\n",
"The error is the algebraic epipolar error $ (K_B^{-1} p_B)^T E (K_A^{-1} p_A) $.\n",
"\n",
"**Note:** Like Factor4, this is often ill-posed without strong priors."
]
},
{
"cell_type": "code",
"execution_count": 13,
"metadata": {
"id": "factor5_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Factor 5: keys = { e0 k0 k1 }\n",
"isotropic dim=1 sigma=0.01\n",
" EssentialMatrixFactor5 with measurements\n",
" (480 288)' and (464 312)'\n",
"\n",
"Error for Factor 5: 11.520000000000007\n"
]
}
],
"source": [
"# Keys for potentially different calibrations\n",
"keyKA = K(0) # Can reuse keyK if they are actually the same\n",
"keyKB = K(1)\n",
"\n",
"factor5 = EssentialMatrixFactor5Cal3_S2(keyE, keyKA, keyKB, pA_pixels, pB_pixels, epipolar_noise)\n",
"factor5.print(\"\\nFactor 5: \")\n",
"\n",
"# Evaluate error (requires E, KA, KB)\n",
"# values already contains E(0) and K(0)\n",
"values.insert(keyKB, K_cal) # Assume KB is also the same known K\n",
"error5 = factor5.error(values)\n",
"print(f\"\\nError for Factor 5: {error5}\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

269
gtsam/slam/doc/FrobeniusFactor.ipynb Normal file
View File

@ -0,0 +1,269 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# Frobenius Norm Factors"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"This header defines factors that operate directly on the entries of rotation matrices (`Rot3` or generally `SO(n)`) rather than using their Lie algebra representation (log map). They minimize the Frobenius norm of the difference between rotation matrices.\n",
"\n",
"These factors can sometimes be useful in specific optimization contexts, particularly in rotation averaging problems or as alternatives to standard `BetweenFactor` or `PriorFactor` on rotations.\n",
"\n",
"* `FrobeniusPrior<T>`: Penalizes the Frobenius norm difference between a variable rotation `T` and a fixed target matrix `M`. Error is $||T - M||_F^2$.\n",
"* `FrobeniusFactor<T>`: Penalizes the Frobenius norm difference between two variable rotations `T1` and `T2`. Error is $||T1 - T2||_F^2$.\n",
"* `FrobeniusBetweenFactor<T>`: Penalizes the Frobenius norm difference between the predicted rotation `T2` and the expected rotation `T1 * T12_measured`. Error is $||T1 \\cdot T_{12} - T2||_F^2$.\n",
"**Note:** The noise models for these factors operate on the vectorized rotation matrix (e.g., 9 elements for `Rot3`). The helper function `ConvertNoiseModel` attempts to convert standard rotation noise models (like those for `BetweenFactor<Rot3>`) into an appropriate isotropic noise model for the Frobenius factor. It expects the input noise model to be isotropic."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/FrobeniusFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Rot3, Pose3, Values\n",
"from gtsam import FrobeniusPriorRot3, FrobeniusFactorRot3, FrobeniusBetweenFactorRot3\n",
"from gtsam import symbol_shorthand\n",
"\n",
"X = symbol_shorthand.X\n",
"R = symbol_shorthand.R # Using 'R' for Rot3 keys"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "fprior_header_md"
},
"source": [
"## 1. `FrobeniusPrior<Rot3>`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "fprior_desc_md"
},
"source": [
"Constrains a `Rot3` variable `R(0)` to be close to a target matrix `M` in the Frobenius norm sense."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "fprior_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"FrobeniusPriorRot3: FrobeniusPriorFactor on R0\n",
"Error model: diagonal sigmas [0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01; 0.01];\n",
"\n",
"FrobeniusPrior error (vectorized matrix diff): (9,)\n",
"[ 0.00054931 -0.00997917 0. -0.00997917 -0.00054931 0.\n",
" 0. 0. 0. ]\n"
]
}
],
"source": [
"target_matrix = Rot3.Yaw(0.1).matrix() # Target matrix (must be 3x3)\n",
"key = R(0)\n",
"\n",
"# Create a standard isotropic noise model for rotation (3 dimensional)\n",
"rot_noise_model = gtsam.noiseModel.Isotropic.Sigma(3, 0.01)\n",
"\n",
"# Convert it for the Frobenius factor (9 dimensional)\n",
"frobenius_noise_model_prior = gtsam.noiseModel.Isotropic.Sigma(9, 0.01) # Or use ConvertNoiseModel\n",
"\n",
"prior_fro = FrobeniusPriorRot3(key, target_matrix, frobenius_noise_model_prior)\n",
"prior_fro.print(\"FrobeniusPriorRot3: \")\n",
"\n",
"# Evaluate error\n",
"values = Values()\n",
"values.insert(key, Rot3.Yaw(0.11)) # Slightly different rotation\n",
"error_prior = prior_fro.error(values)\n",
"print(f\"\\nFrobeniusPrior error (vectorized matrix diff): {error_prior.shape}\\n{error_prior}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "ffactor_header_md"
},
"source": [
"## 1. `FrobeniusFactor<Rot3>`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "ffactor_desc_md"
},
"source": [
"Constrains two `Rot3` variables `R(0)` and `R(1)` to be close to each other in the Frobenius norm sense."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "ffactor_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"FrobeniusFactorRot3: keys = { r0 r1 }\n",
" noise model: unit (9) \n"
]
},
{
"ename": "RuntimeError",
"evalue": "Attempting to at the key \"r0\", which does not exist in the Values.",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mRuntimeError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[6], line 13\u001b[0m\n\u001b[0;32m 11\u001b[0m \u001b[38;5;66;03m# Evaluate error\u001b[39;00m\n\u001b[0;32m 12\u001b[0m values\u001b[38;5;241m.\u001b[39minsert(key2, Rot3\u001b[38;5;241m.\u001b[39mYaw(\u001b[38;5;241m0.115\u001b[39m)) \u001b[38;5;66;03m# R1 slightly different from R0\u001b[39;00m\n\u001b[1;32m---> 13\u001b[0m error_factor \u001b[38;5;241m=\u001b[39m \u001b[43mfactor_fro\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 14\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;124mFrobeniusFactor error (vectorized matrix diff): \u001b[39m\u001b[38;5;132;01m{\u001b[39;00merror_factor\u001b[38;5;241m.\u001b[39mshape\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;132;01m{\u001b[39;00merror_factor\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n",
"\u001b[1;31mRuntimeError\u001b[0m: Attempting to at the key \"r0\", which does not exist in the Values."
]
}
],
"source": [
"values = Values()\n",
"\n",
"key1 = R(0)\n",
"key2 = R(1)\n",
"# Use same noise model dimension (9)\n",
"frobenius_noise_model_between = gtsam.noiseModel.Isotropic.Sigma(9, 0.02)\n",
"\n",
"factor_fro = FrobeniusFactorRot3(key1, key2, frobenius_noise_model_between)\n",
"factor_fro.print(\"\\nFrobeniusFactorRot3: \")\n",
"\n",
"# Evaluate error\n",
"values.insert(key2, Rot3.Yaw(0.115)) # R1 slightly different from R0\n",
"error_factor = factor_fro.error(values)\n",
"print(f\"\\nFrobeniusFactor error (vectorized matrix diff): {error_factor.shape}\\n{error_factor}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "fbetween_header_md"
},
"source": [
"## 3. `FrobeniusBetweenFactor<Rot3>`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "fbetween_desc_md"
},
"source": [
"Acts like `BetweenFactor<Rot3>` but minimizes $||R_1 \\cdot R_{12} - R_2||_F^2$ instead of using the Log map error."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "fbetween_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"FrobeniusBetweenFactorRot3: FrobeniusBetweenFactor<gtsam::Rot3>(R0,R1)\n",
" T12: R: [\n",
"\t0.999875, -0.0149991, 0;\n",
"\t0.0149991, 0.999875, 0;\n",
"\t0, 0, 1\n",
"]\n",
"\n",
" noise model: diagonal sigmas [0.005; 0.005; 0.005; 0.005; 0.005; 0.005; 0.005; 0.005; 0.005];\n",
"\n",
"FrobeniusBetweenFactor error: (9,)\n",
"[-0. 0. 0. 0. -0. 0. 0. 0. 0.]\n"
]
}
],
"source": [
"measured_R12 = Rot3.Yaw(0.005)\n",
"# Use same noise model dimension (9)\n",
"frobenius_noise_model_b = gtsam.noiseModel.Isotropic.Sigma(9, 0.005)\n",
"\n",
"between_fro = FrobeniusBetweenFactorRot3(key1, key2, measured_R12, frobenius_noise_model_b)\n",
"between_fro.print(\"\\nFrobeniusBetweenFactorRot3: \")\n",
"\n",
"# Evaluate error (uses R(0)=Yaw(0.11), R(1)=Yaw(0.115))\n",
"error_between = between_fro.error(values)\n",
"print(f\"\\nFrobeniusBetweenFactor error: {error_between.shape}\\n{error_between}\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.11.9"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

223
gtsam/slam/doc/GeneralSFMFactor.ipynb Normal file
View File

@ -0,0 +1,223 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# GeneralSFMFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"This header defines factors for Structure from Motion (SfM) problems where camera calibration might be unknown or optimized alongside pose and structure.\n",
"\n",
"`GeneralSFMFactor<CAMERA, LANDMARK>`:\n",
"- A binary factor connecting a `CAMERA` variable and a `LANDMARK` variable.\n",
"- Represents the reprojection error of the `LANDMARK` into the `CAMERA` view, compared to a 2D `measured_` pixel coordinate.\n",
"- The `CAMERA` type encapsulates both pose and calibration (e.g., `PinholeCamera<Cal3Bundler>`).\n",
"- Error: `camera.project(landmark) - measured`\n",
"\n",
"`GeneralSFMFactor2<CALIBRATION>`:\n",
"- A ternary factor connecting a `Pose3` variable, a `Point3` landmark variable, and a `CALIBRATION` variable.\n",
"- This explicitly separates the camera pose and calibration into different variables.\n",
"- Error: `PinholeCamera<CALIBRATION>(pose, calibration).project(landmark) - measured`\n",
"\n",
"These factors are core components for visual SLAM or SfM systems where calibration is refined or initially unknown."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/GeneralSFMFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import (GeneralSFMFactorCal3_S2, GeneralSFMFactor2Cal3_S2,\n",
" PinholeCameraCal3_S2, Pose3, Point3, Point2, Cal3_S2, Values)\n",
"from gtsam import symbol_shorthand\n",
"\n",
"X = symbol_shorthand.X\n",
"L = symbol_shorthand.L\n",
"K = symbol_shorthand.K\n",
"C = symbol_shorthand.C # For Camera variable"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor1_header_md"
},
"source": [
"## 1. `GeneralSFMFactor<CAMERA, LANDMARK>`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor1_desc_md"
},
"source": [
"Connects a combined Camera variable (pose+calibration) and a Landmark.\n",
"Requires the `Values` object to contain instances of the specific `CAMERA` type (e.g., `PinholeCameraCal3_S2`)."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "factor1_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"GeneralSFMFactor: keys = { c0 l0 }\n",
" noise model: unit (2) \n",
"GeneralSFMFactor: .z[\n",
"\t320;\n",
"\t240\n",
"]\n",
"\n",
"Error for GeneralSFMFactor: 0.0\n"
]
}
],
"source": [
"measured_pt = Point2(320, 240) # Measurement in pixels\n",
"sfm_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
"camera_key = C(0)\n",
"landmark_key = L(0)\n",
"\n",
"# Note: The factor type name includes the Calibration, e.g., GeneralSFMFactorCal3_S2\n",
"factor1 = GeneralSFMFactorCal3_S2(measured_pt, sfm_noise, camera_key, landmark_key)\n",
"factor1.print(\"GeneralSFMFactor: \")\n",
"\n",
"# Evaluate error - requires a Camera object in Values\n",
"values = Values()\n",
"camera_pose = Pose3() # Identity pose\n",
"calibration = Cal3_S2(500, 500, 0, 320, 240) # fx, fy, s, u0, v0\n",
"camera = PinholeCameraCal3_S2(camera_pose, calibration)\n",
"landmark = Point3(0, 0, 5) # Point 5m in front of camera\n",
"\n",
"values.insert(camera_key, camera)\n",
"values.insert(landmark_key, landmark)\n",
"\n",
"error1 = factor1.error(values)\n",
"print(f\"\\nError for GeneralSFMFactor: {error1}\") # Should be [0, 0] if landmark projects to measured_pt"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor2_header_md"
},
"source": [
"## 2. `GeneralSFMFactor2<CALIBRATION>`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor2_desc_md"
},
"source": [
"Connects separate Pose3, Point3 (Landmark), and Calibration variables."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "factor2_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"GeneralSFMFactor2: keys = { x0 l0 k0 }\n",
" noise model: unit (2) \n",
"GeneralSFMFactor2: .z[\n",
"\t320;\n",
"\t240\n",
"]\n",
"\n",
"Error for GeneralSFMFactor2: 0.0\n"
]
}
],
"source": [
"pose_key = X(0)\n",
"calib_key = K(0)\n",
"# landmark_key = L(0) # Re-use from above\n",
"\n",
"# Note: The factor type name includes the Calibration, e.g., GeneralSFMFactor2Cal3_S2\n",
"factor2 = GeneralSFMFactor2Cal3_S2(measured_pt, sfm_noise, pose_key, landmark_key, calib_key)\n",
"factor2.print(\"GeneralSFMFactor2: \")\n",
"\n",
"# Evaluate error - requires Pose3, Point3, Cal3_S2 objects in Values\n",
"values2 = Values()\n",
"values2.insert(pose_key, camera_pose)\n",
"values2.insert(landmark_key, landmark)\n",
"values2.insert(calib_key, calibration)\n",
"\n",
"error2 = factor2.error(values2)\n",
"print(f\"\\nError for GeneralSFMFactor2: {error2}\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

256
gtsam/slam/doc/InitializePose3.ipynb Normal file
View File

@ -0,0 +1,256 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# InitializePose3"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"The `InitializePose3` structure provides static methods for computing an initial estimate for 3D poses (`Pose3`) in a factor graph, particularly useful for Structure from Motion (SfM) or SLAM problems.\n",
"The core idea is to first estimate the orientations (`Rot3`) independently and then use these estimates to initialize a linear system for the translations.\n",
"\n",
"Key static methods:\n",
"* `buildPose3graph(graph)`: Extracts the subgraph containing only `Pose3` `BetweenFactor` and `PriorFactor` constraints from a larger `NonlinearFactorGraph`.\n",
"* `computeOrientationsChordal(pose3Graph)`: Estimates rotations using chordal relaxation on the rotation constraints.\n",
"* `computeOrientationsGradient(pose3Graph, initialGuess)`: Estimates rotations using gradient descent on the manifold.\n",
"* `initializeOrientations(graph)`: Convenience function combining `buildPose3graph` and `computeOrientationsChordal`.\n",
"* `computePoses(initialRot, poseGraph)`: Computes translations given estimated rotations by solving a linear system (performing one Gauss-Newton iteration on poses).\n",
"* `initialize(graph)`: Performs the full initialization pipeline (extract graph, estimate rotations via chordal, compute translations).\n",
"* `initialize(graph, givenGuess, useGradient)`: Full pipeline allowing specification of an initial guess and choosing between chordal or gradient descent for rotations."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/InitializePose3.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import NonlinearFactorGraph, Values, Pose3, Rot3, Point3, PriorFactorPose3, BetweenFactorPose3\n",
"from gtsam import InitializePose3\n",
"from gtsam import symbol_shorthand\n",
"\n",
"X = symbol_shorthand.X"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "example_header_md"
},
"source": [
"## Example Initialization Pipeline"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "example_desc_md"
},
"source": [
"We'll create a simple 3D pose graph and use the `InitializePose3.initialize` method to get an initial estimate."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "example_pipeline_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Original Factor Graph:\n",
"size: 4\n",
"\n",
"Factor 0: PriorFactor on x0\n",
" prior mean: R: [\n",
"\t0.995004, -0.0998334, 0;\n",
"\t0.0998334, 0.995004, 0;\n",
"\t0, 0, 1\n",
"]\n",
"t: 0.1 0 0\n",
" noise model: diagonal sigmas [0.01; 0.01; 0.01; 0.05; 0.05; 0.05];\n",
"\n",
"Factor 1: BetweenFactor(x0,x1)\n",
" measured: R: [\n",
"\t0.877582562, -0.479425539, 0;\n",
"\t0.479425539, 0.877582562, 0;\n",
"\t0, 0, 1\n",
"]\n",
"t: 1 0.1 0\n",
" noise model: diagonal sigmas [0.05; 0.05; 0.05; 0.2; 0.2; 0.2];\n",
"\n",
"Factor 2: BetweenFactor(x1,x2)\n",
" measured: R: [\n",
"\t0.921060994, -0.389418342, 0;\n",
"\t0.389418342, 0.921060994, 0;\n",
"\t0, 0, 1\n",
"]\n",
"t: 0.9 -0.1 0\n",
" noise model: diagonal sigmas [0.05; 0.05; 0.05; 0.2; 0.2; 0.2];\n",
"\n",
"Factor 3: BetweenFactor(x2,x0)\n",
" measured: R: [\n",
"\t0.621609968, 0.78332691, 0;\n",
"\t-0.78332691, 0.621609968, 0;\n",
"\t0, 0, 1\n",
"]\n",
"t: -1.8 0.05 0\n",
" noise model: diagonal sigmas [0.1; 0.1; 0.1; 0.4; 0.4; 0.4];\n"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"\n",
"Initial Estimate (using InitializePose3.initialize):\n",
"\n",
"Values with 3 values:\n",
"Value x0: (class gtsam::Pose3)\n",
"R: [\n",
"\t0.995004165, -0.0998334166, 0;\n",
"\t0.0998334166, 0.995004165, 0;\n",
"\t0, 0, 1\n",
"]\n",
"t: 0.1 -7.63278329e-15 0\n",
"\n",
"Value x1: (class gtsam::Pose3)\n",
"R: [\n",
"\t0.825335615, -0.564642473, 0;\n",
"\t0.564642473, 0.825335615, 0;\n",
"\t0, 0, 1\n",
"]\n",
"t: 0.956742586 0.343109526 0\n",
"\n",
"Value x2: (class gtsam::Pose3)\n",
"R: [\n",
"\t0.540302306, -0.841470985, 0;\n",
"\t0.841470985, 0.540302306, 0;\n",
"\t0, 0, 1\n",
"]\n",
"t: 1.62773065 0.912529884 0\n",
"\n"
]
}
],
"source": [
"# 1. Create a NonlinearFactorGraph with Pose3 factors\n",
"graph = NonlinearFactorGraph()\n",
"\n",
"# Add a prior on the first pose\n",
"prior_mean = Pose3(Rot3.Yaw(0.1), Point3(0.1, 0, 0))\n",
"prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.01]*3 + [0.05]*3))\n",
"graph.add(PriorFactorPose3(X(0), prior_mean, prior_noise))\n",
"\n",
"# Add odometry factors\n",
"odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05]*3 + [0.2]*3))\n",
"odometry1 = Pose3(Rot3.Yaw(0.5), Point3(1.0, 0.1, 0))\n",
"odometry2 = Pose3(Rot3.Yaw(0.4), Point3(0.9, -0.1, 0))\n",
"graph.add(BetweenFactorPose3(X(0), X(1), odometry1, odometry_noise))\n",
"graph.add(BetweenFactorPose3(X(1), X(2), odometry2, odometry_noise))\n",
"\n",
"# Add a loop closure factor (less certain)\n",
"loop_mean = Pose3(Rot3.Yaw(-0.9), Point3(-1.8, 0.05, 0))\n",
"loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1]*3 + [0.4]*3))\n",
"graph.add(BetweenFactorPose3(X(2), X(0), loop_mean, loop_noise))\n",
"\n",
"graph.print(\"Original Factor Graph:\\n\")\n",
"\n",
"# 2. Perform initialization using the default chordal relaxation method\n",
"initial_estimate = InitializePose3.initialize(graph)\n",
"\n",
"print(\"\\nInitial Estimate (using InitializePose3.initialize):\\n\")\n",
"initial_estimate.print()\n",
"\n",
"# 3. (Optional) Perform initialization step-by-step\n",
"# pose3graph = InitializePose3.buildPose3graph(graph)\n",
"# initial_orientations = InitializePose3.initializeOrientations(graph)\n",
"# initial_estimate_manual = InitializePose3.computePoses(initial_orientations, pose3graph)\n",
"# print(\"\\nInitial Estimate (Manual Steps):\\n\")\n",
"# initial_estimate_manual.print()"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "notes_header_md"
},
"source": [
"## Notes"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "notes_desc_md"
},
"source": [
"- The quality of the initial estimate depends heavily on the quality and connectivity of the pose graph factors.\n",
"- Chordal relaxation (`computeOrientationsChordal`) is generally faster and often sufficient.\n",
"- Gradient descent (`computeOrientationsGradient`) might provide slightly more accurate orientations but is slower and requires a good initial guess.\n",
"- The final `computePoses` step performs only a *single* Gauss-Newton iteration, assuming the rotations are fixed. The resulting estimate is meant as an initialization for a full nonlinear optimization (e.g., using `GaussNewtonOptimizer` or `LevenbergMarquardtOptimizer`)."
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

245
gtsam/slam/doc/KarcherMeanFactor.ipynb Normal file
View File

@ -0,0 +1,245 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# Karcher Mean Factors"
]
},
{
"cell_type": "markdown",
"metadata": {
"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 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",
"* `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."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/KarcherMeanFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"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"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "find_header_md"
},
"source": [
"## 1. `FindKarcherMean`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "find_desc_md"
},
"source": [
"Computes the Karcher mean of a list of rotations."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "find_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Input Rotations (Yaw angles):\n",
" 0.100\n",
" 0.150\n",
" 0.050\n",
" 0.120\n",
"\n",
"Computed Karcher Mean (Yaw angle): 0.105\n"
]
}
],
"source": [
"# Create a list of Rot3 objects\n",
"rotations = gtsam.Rot3Vector()\n",
"rotations.append(Rot3.Yaw(0.1))\n",
"rotations.append(Rot3.Yaw(0.15))\n",
"rotations.append(Rot3.Yaw(0.05))\n",
"rotations.append(Rot3.Yaw(0.12))\n",
"\n",
"# Compute the Karcher mean\n",
"karcher_mean = FindKarcherMean(rotations)\n",
"\n",
"print(\"Input Rotations (Yaw angles):\")\n",
"for r in rotations: print(f\" {r.yaw():.3f}\")\n",
"\n",
"print(f\"\\nComputed Karcher Mean (Yaw angle): {karcher_mean.yaw():.3f}\")\n",
"# Note: For yaw rotations, the Karcher mean yaw is close to the arithmetic mean (0.105)"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor_header_md"
},
"source": [
"## 2. `KarcherMeanFactor<Rot3>`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor_desc_md"
},
"source": [
"Creates a factor that constrains the rotational average of a set of `Rot3` variables.\n",
"It acts as a soft gauge constraint. When linearized, it yields a Jacobian factor where each block corresponding to a variable is $\\sqrt{\\beta} I_{3x3}$, and the error vector is zero. The `beta` parameter (optional, defaults to 1) controls the strength (precision) of the constraint."
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {
"id": "factor_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"KarcherMeanFactorRot3: keys = { r0 r1 r2 }\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'"
]
}
],
"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.print(\"KarcherMeanFactorRot3: \")\n",
"\n",
"# Linearization example\n",
"values = Values()\n",
"values.insert(R(0), Rot3.Yaw(0.1))\n",
"values.insert(R(1), Rot3.Yaw(0.2))\n",
"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",
"b = linearized_factor.getb()\n",
"\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))"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

230
gtsam/slam/doc/OrientedPlane3Factor.ipynb Normal file
View File

@ -0,0 +1,230 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# OrientedPlane3 Factors"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"This header defines factors for working with planar landmarks represented by `gtsam.OrientedPlane3`.\n",
"`OrientedPlane3` represents a plane using normalized coefficients $(n_x, n_y, n_z, d)$, where $(n_x, n_y, n_z)$ is the unit normal vector and $d$ is the distance from the origin along the normal.\n",
"\n",
"Factors defined:\n",
"* `OrientedPlane3Factor`: A binary factor connecting a `Pose3` and an `OrientedPlane3`. It measures the difference between the plane parameters as observed from the given pose and a measured plane.\n",
"* `OrientedPlane3DirectionPrior`: A unary factor on an `OrientedPlane3`. It penalizes the difference between the plane's normal direction and a measured direction (represented by the normal of a measured `OrientedPlane3`). **Note:** The factor error is 3D, but only constrains 2 degrees of freedom (direction). Consider using a more specific direction factor if only direction is measured."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/OrientedPlane3Factor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [
{
"ename": "ImportError",
"evalue": "cannot import name 'OrientedPlane3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, OrientedPlane3, Point3, Rot3, Values\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m OrientedPlane3Factor, OrientedPlane3DirectionPrior\n\u001b[0;32m 5\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n",
"\u001b[1;31mImportError\u001b[0m: cannot import name 'OrientedPlane3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
]
}
],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Pose3, OrientedPlane3, Point3, Rot3, Values\n",
"from gtsam import OrientedPlane3Factor, OrientedPlane3DirectionPrior\n",
"from gtsam import symbol_shorthand\n",
"\n",
"X = symbol_shorthand.X\n",
"P = symbol_shorthand.P # For Plane"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor_header_md"
},
"source": [
"## 1. `OrientedPlane3Factor`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor_desc_md"
},
"source": [
"Connects a `Pose3` (camera/robot pose) and an `OrientedPlane3` (landmark). The measurement is the plane as observed *from the sensor frame*.\n",
"The error is calculated by transforming the global plane landmark into the sensor frame defined by the pose, and then computing the difference (`localCoordinates`) with the measured plane."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "factor_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"OrientedPlane3Factor: OrientedPlane3Factor Factor with keys: x0 P0\n",
"\n",
"Error model: diagonal sigmas [0.05; 0.05; 0.05];\n",
"\n",
"Error at ground truth: [-0. -0. -0.]\n",
"\n",
"Error with noisy plane: [ -0.0150837 -0.00503354 -49.83283361]\n"
]
}
],
"source": [
"# Ground truth plane (e.g., z=1 in world frame)\n",
"gt_plane_world = OrientedPlane3(0, 0, 1, 1)\n",
"\n",
"# Ground truth pose\n",
"gt_pose = Pose3(Rot3.Yaw(0.1), Point3(0.5, 0, 0))\n",
"\n",
"# Measurement: transform the world plane into the camera frame\n",
"# measured_plane = gt_plane_world.transform(gt_pose)\n",
"# C++ header: Plane measurement z is a 4D vector [a,b,c,d] coefficients:\n",
"measured_plane_coeffs = gt_plane_world.coeffs()\n",
"plane_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05)\n",
"\n",
"pose_key = X(0)\n",
"plane_key = P(0)\n",
"\n",
"plane_factor = OrientedPlane3Factor(measured_plane_coeffs, plane_noise, pose_key, plane_key)\n",
"plane_factor.print(\"OrientedPlane3Factor: \")\n",
"\n",
"# Evaluate error\n",
"values = Values()\n",
"values.insert(pose_key, gt_pose)\n",
"values.insert(plane_key, gt_plane_world)\n",
"error1 = plane_factor.error(values)\n",
"print(f\"\\nError at ground truth: {error1}\")\n",
"\n",
"# Evaluate with slightly different plane estimate\n",
"noisy_plane = OrientedPlane3(0.01, 0.01, 0.99, 1.05)\n",
"values.update(plane_key, noisy_plane)\n",
"error2 = plane_factor.error(values)\n",
"print(f\"\\nError with noisy plane: {error2}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "prior_header_md"
},
"source": [
"## 2. `OrientedPlane3DirectionPrior`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "prior_desc_md"
},
"source": [
"A unary factor that puts a prior on the *direction* (normal vector) of an `OrientedPlane3`. The distance component (`d`) of the measured plane is ignored.\n",
"The error is the 3D difference between the estimated plane's normal and the measured plane's normal."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "prior_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"OrientedPlane3DirectionPrior: Factor OrientedPlane3DirectionPrior on P0\n",
"Noise model: diagonal sigmas [0.02; 0.02; 0.02];\n",
"\n",
"Error for prior on noisy_plane: [-0.5 -0.5 0.5]\n",
"Error for prior on gt_plane_world: [0. 0. 0.]\n"
]
}
],
"source": [
"# Measured direction prior (e.g., plane normal is close to world Z axis)\n",
"measured_prior_plane = OrientedPlane3(0, 0, 1, 0) # Distance (last coeff) is ignored\n",
"direction_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.02)\n",
"\n",
"prior_factor = OrientedPlane3DirectionPrior(plane_key, measured_prior_plane.coeffs(), direction_noise)\n",
"prior_factor.print(\"OrientedPlane3DirectionPrior: \")\n",
"\n",
"# Evaluate error using the 'noisy_plane' from the previous step\n",
"error_prior = prior_factor.error(values) # values still contains plane_key -> noisy_plane\n",
"print(f\"\\nError for prior on noisy_plane: {error_prior}\")\n",
"\n",
"# Evaluate error for ground truth plane\n",
"values.update(plane_key, gt_plane_world)\n",
"error_prior_gt = prior_factor.error(values)\n",
"print(f\"Error for prior on gt_plane_world: {error_prior_gt}\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

293
gtsam/slam/doc/PlanarProjectionFactor.ipynb Normal file
View File

@ -0,0 +1,293 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# PlanarProjectionFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"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",
"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 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",
"\n",
"The core projection logic involves converting the `Pose2` `wTb` to a `Pose3` assuming z=0 and yaw=theta, composing it with `bTc` to get the world-to-camera pose `wTc`, and then using a standard `PinholeCamera` model to project the landmark.\n",
"\n",
"Variants:\n",
"* `PlanarProjectionFactor1`: Unknown is robot `Pose2` (`wTb`). Landmark, `bTc`, and calibration are fixed. Useful for localization.\n",
"* `PlanarProjectionFactor2`: Unknowns are robot `Pose2` (`wTb`) and `Point3` landmark. `bTc` and calibration are fixed. Useful for planar SLAM.\n",
"* `PlanarProjectionFactor3`: Unknowns are robot `Pose2` (`wTb`), camera offset `Pose3` (`bTc`), and `Cal3DS2` calibration. Landmark is fixed. Useful for calibration."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/PlanarProjectionFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"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"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor1_header_md"
},
"source": [
"## 1. `PlanarProjectionFactor1` (Localization)"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor1_desc_md"
},
"source": [
"Used when the landmark, camera offset (`bTc`), and calibration (`calib`) are known, and we want to estimate the robot's `Pose2` (`wTb`)."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "factor1_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Ground Truth Pose2: (1, 0, 0.785398)\n",
"\n",
"Calculated Measurement: [ 909.25565099 1841.1002863 ]\n",
"Factor 1: keys = { x0 }\n",
"isotropic dim=2 sigma=1.5\n",
"\n",
"Error at ground truth: 0.0\n",
"Error at noisy pose: 3317.647263749095\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",
"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_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",
"print(f\"Ground Truth Pose2: {gt_pose2}\")\n",
"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.print(\"Factor 1: \")\n",
"\n",
"# Evaluate error\n",
"values = Values()\n",
"values.insert(pose_key, 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",
"error1_noisy = factor1.error(values)\n",
"print(f\"Error at noisy pose: {error1_noisy}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor2_header_md"
},
"source": [
"## 2. `PlanarProjectionFactor2` (Planar SLAM)"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor2_desc_md"
},
"source": [
"Used when the camera offset (`bTc`) and calibration (`calib`) are known, but both the robot `Pose2` (`wTb`) and the `Point3` landmark position are unknown."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {
"id": "factor2_example_code"
},
"outputs": [
{
"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>"
]
}
],
"source": [
"landmark_key = L(0)\n",
"\n",
"factor2 = PlanarProjectionFactor2(pose_key, landmark_key, measured_pt2, body_T_cam, calib, measurement_noise)\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",
"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",
"error2_noisy = factor2.error(values)\n",
"print(f\"Error with noisy landmark: {error2_noisy}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor3_header_md"
},
"source": [
"## 3. `PlanarProjectionFactor3` (Calibration)"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor3_desc_md"
},
"source": [
"Used when the landmark position is known, but the robot `Pose2` (`wTb`), the camera offset `Pose3` (`bTc`), and the `Cal3DS2` calibration are unknown."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "factor3_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Factor 3: Factor NoiseModelFactor3 on x0 O0 C0\n",
"Noise model: diagonal sigmas [1.5; 1.5];\n",
"\n",
"Error at ground truth: [-0. -0.]\n",
"Error with noisy calibration: [8.38867847 0.63659684]\n"
]
}
],
"source": [
"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.print(\"Factor 3: \")\n",
"\n",
"# Evaluate error\n",
"values = Values()\n",
"values.insert(pose_key, 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",
"print(f\"\\nError at ground truth: {error3_gt}\")\n",
"\n",
"noisy_calib = Cal3DS2(fx=510, fy=495, s=0, u0=322, v0=241, k1=0, k2=0, p1=0, p2=0)\n",
"values.update(calib_key, noisy_calib)\n",
"error3_noisy = factor3.error(values)\n",
"print(f\"Error with noisy calibration: {error3_noisy}\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

215
gtsam/slam/doc/PoseRotationPrior.ipynb Normal file
View File

@ -0,0 +1,215 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# PoseRotationPrior"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`PoseRotationPrior<POSE>` is a unary factor that applies a prior constraint only to the **rotation** component of a `POSE` variable (e.g., `Pose2` or `Pose3`).\n",
"It ignores the translation component of the pose variable during error calculation.\n",
"The error is calculated as the difference between the rotation component of the pose variable and the measured prior rotation, expressed in the tangent space of the rotation group.\n",
"\n",
"Error: $ \\text{Log}(\\text{measured}^{-1} \\cdot \\text{pose.rotation}()) $\n",
"\n",
"This is useful when you have information about the absolute orientation of a pose but little or no information about its translation."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/PoseRotationPrior.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "imports_code"
},
"outputs": [
{
"ename": "ImportError",
"evalue": "cannot import name 'PoseRotationPriorPose' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[2], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, Rot3, Point3, Values, PoseRotationPriorPose\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 6\u001b[0m X \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mX\n",
"\u001b[1;31mImportError\u001b[0m: cannot import name 'PoseRotationPriorPose' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
]
}
],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Pose3, Rot3, Point3, Values, PoseRotationPriorPose3\n",
"from gtsam import symbol_shorthand\n",
"\n",
"X = symbol_shorthand.X"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_header_md"
},
"source": [
"## Creating a PoseRotationPrior"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_desc_md"
},
"source": [
"Provide the key of the pose variable, the measured prior rotation (`Rot3` for `Pose3`, `Rot2` for `Pose2`), and a noise model defined on the rotation manifold's dimension (e.g., 3 for `Rot3`)."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"PoseRotationPrior: PoseRotationPriorFactor on x0\n",
"Noise model: diagonal sigmas [0.05; 0.05; 0.05];\n",
"Measured Rotation R: [\n",
"\t0.707107, -0.707107, 0;\n",
"\t0.707107, 0.707107, 0;\n",
"\t0, 0, 1\n",
"]\n",
"\n"
]
}
],
"source": [
"pose_key = X(0)\n",
"measured_rotation = Rot3.Yaw(np.pi / 4) # Prior belief about orientation\n",
"\n",
"# Noise model on rotation (3 dimensions for Rot3)\n",
"rotation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.05) # 0.05 radians std dev\n",
"\n",
"# Factor type includes the Pose type, e.g. PoseRotationPriorPose3\n",
"factor = PoseRotationPriorPose3(pose_key, measured_rotation, rotation_noise)\n",
"factor.print(\"PoseRotationPrior: \")\n",
"\n",
"# Alternative constructor: extract rotation from a full Pose3 prior\n",
"full_pose_prior = Pose3(measured_rotation, Point3(10, 20, 30)) # Translation is ignored\n",
"factor_from_pose = PoseRotationPriorPose3(pose_key, full_pose_prior, rotation_noise)\n",
"# factor_from_pose.print(\"\\nFrom Pose Prior: \") # Should be identical"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_header_md"
},
"source": [
"## Evaluating the Error"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_desc_md"
},
"source": [
"The error depends only on the rotation part of the `Pose3` value in the `Values` object."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Error with correct rotation: [0. 0. 0.] (Should be near zero)\n",
"Error with incorrect rotation: [-0. -0. 2.00000004] (Should be non-zero)\n",
"Error with different translation: [-0. -0. 2.00000004] (Should be same as error2)\n"
]
}
],
"source": [
"values = Values()\n",
"\n",
"# Pose with correct rotation but different translation\n",
"pose_val1 = Pose3(measured_rotation, Point3(1, 2, 3))\n",
"values.insert(pose_key, pose_val1)\n",
"error1 = factor.error(values)\n",
"print(f\"Error with correct rotation: {error1} (Should be near zero)\")\n",
"\n",
"# Pose with incorrect rotation\n",
"pose_val2 = Pose3(Rot3.Yaw(np.pi / 4 + 0.1), Point3(1, 2, 3))\n",
"values.update(pose_key, pose_val2)\n",
"error2 = factor.error(values)\n",
"print(f\"Error with incorrect rotation: {error2} (Should be non-zero)\")\n",
"\n",
"# Check that translation change doesn't affect error\n",
"pose_val3 = Pose3(Rot3.Yaw(np.pi / 4 + 0.1), Point3(100, 200, 300))\n",
"values.update(pose_key, pose_val3)\n",
"error3 = factor.error(values)\n",
"print(f\"Error with different translation: {error3} (Should be same as error2)\")\n",
"assert np.allclose(error2, error3)"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

214
gtsam/slam/doc/PoseTranslationPrior.ipynb Normal file
View File

@ -0,0 +1,214 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# PoseTranslationPrior"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`PoseTranslationPrior<POSE>` is a unary factor that applies a prior constraint only to the **translation** component of a `POSE` variable (e.g., `Pose2` or `Pose3`).\n",
"It ignores the rotation component of the pose variable during error calculation.\n",
"The error is calculated as the difference between the translation component of the pose variable and the measured prior translation, expressed in the tangent space (which is typically just vector subtraction for `Point2` or `Point3`).\n",
"\n",
"Error: $ \\text{pose.translation}() - \\text{measured} $ (potentially rotated into world frame if translation is in body frame - check `evaluateError` implementation details in C++). *Correction based on C++ code*: The error is `traits<Translation>::Local(measured_, pose.translation())`, calculated in the world frame, and the Jacobian involves the pose's rotation.\n",
"\n",
"This is useful when you have information about the absolute position of a pose but little or no information about its orientation (e.g., GPS measurement)."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/PoseTranslationPrior.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [
{
"ename": "ImportError",
"evalue": "cannot import name 'PoseTranslationPriorPose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, Rot3, Point3, Values, PoseTranslationPriorPose3\n\u001b[0;32m 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 6\u001b[0m X \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mX\n",
"\u001b[1;31mImportError\u001b[0m: cannot import name 'PoseTranslationPriorPose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
]
}
],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Pose3, Rot3, Point3, Values, PoseTranslationPriorPose3\n",
"from gtsam import symbol_shorthand\n",
"\n",
"X = symbol_shorthand.X"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_header_md"
},
"source": [
"## Creating a PoseTranslationPrior"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_desc_md"
},
"source": [
"Provide the key of the pose variable, the measured prior translation (`Point3` for `Pose3`, `Point2` for `Pose2`), and a noise model defined on the translation space dimension (e.g., 3 for `Point3`)."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"PoseTranslationPrior: Factor PoseTranslationPrior on x0\n",
"Noise model: diagonal sigmas [0.5; 0.5; 0.5];\n",
"Measured Translation10 20 5\n"
]
}
],
"source": [
"pose_key = X(0)\n",
"measured_translation = Point3(10.0, 20.0, 5.0) # Prior belief about position\n",
"\n",
"# Noise model on translation (3 dimensions for Point3)\n",
"translation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.5) # 0.5 meters std dev\n",
"\n",
"# Factor type includes the Pose type, e.g. PoseTranslationPriorPose3\n",
"factor = PoseTranslationPriorPose3(pose_key, measured_translation, translation_noise)\n",
"factor.print(\"PoseTranslationPrior: \")\n",
"\n",
"# Alternative constructor: extract translation from a full Pose3 prior\n",
"full_pose_prior = Pose3(Rot3.Yaw(np.pi/2), measured_translation) # Rotation is ignored\n",
"factor_from_pose = PoseTranslationPriorPose3(pose_key, full_pose_prior, translation_noise)\n",
"# factor_from_pose.print(\"\\nFrom Pose Prior: \") # Should be identical"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_header_md"
},
"source": [
"## Evaluating the Error"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_desc_md"
},
"source": [
"The error depends only on the translation part of the `Pose3` value in the `Values` object."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Error with correct translation: [0. 0. 0.] (Should be near zero)\n",
"Error with incorrect translation: [ 0.39999999 -0.19999999 0.19999999] (Should be non-zero)\n",
"Error with different rotation: [ 0.39999999 -0.19999999 0.19999999] (Should reflect Jacobian change)\n",
"Unwhitened error with different rotation: [ 0.2 -0.1 0.1] (Should be [0.2, -0.1, 0.1])\n"
]
}
],
"source": [
"values = Values()\n",
"\n",
"# Pose with correct translation but different rotation\n",
"pose_val1 = Pose3(Rot3.Roll(0.5), measured_translation)\n",
"values.insert(pose_key, pose_val1)\n",
"error1 = factor.error(values)\n",
"print(f\"Error with correct translation: {error1} (Should be near zero)\")\n",
"\n",
"# Pose with incorrect translation\n",
"pose_val2 = Pose3(Rot3.Roll(0.5), Point3(10.2, 19.9, 5.1))\n",
"values.update(pose_key, pose_val2)\n",
"error2 = factor.error(values)\n",
"print(f\"Error with incorrect translation: {error2} (Should be non-zero)\")\n",
"\n",
"# Check that rotation change doesn't affect unwhitened error\n",
"pose_val3 = Pose3(Rot3.Yaw(1.0), Point3(10.2, 19.9, 5.1))\n",
"values.update(pose_key, pose_val3)\n",
"error3 = factor.error(values)\n",
"unwhitened2 = factor.unwhitenedError(values)\n",
"print(f\"Error with different rotation: {error3} (Should reflect Jacobian change)\")\n",
"print(f\"Unwhitened error with different rotation: {unwhitened2} (Should be [0.2, -0.1, 0.1])\")\n",
"# assert np.allclose(error2, error3) # Whitened error WILL change due to Jacobian\n",
"assert np.allclose(unwhitened2, np.array([0.2, -0.1, 0.1]))"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

238
gtsam/slam/doc/ProjectionFactor.ipynb Normal file
View File

@ -0,0 +1,238 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# GenericProjectionFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`GenericProjectionFactor<POSE, LANDMARK, CALIBRATION>` is a versatile factor for monocular camera measurements.\n",
"It models the reprojection error between the predicted projection of a 3D `LANDMARK` (usually `Point3`) onto the image plane of a camera defined by `POSE` (usually `Pose3`) and `CALIBRATION` (e.g., `Cal3_S2`, `Cal3Bundler`, `Cal3DS2`), and a measured 2D pixel coordinate `measured_`.\n",
"\n",
"Key features:\n",
"- **Templated:** Works with different pose, landmark, and calibration types.\n",
"- **Fixed Calibration:** Assumes the `CALIBRATION` object (`K_`) is known and fixed (passed as a shared pointer).\n",
"- **Sensor Offset:** Optionally handles a fixed `body_P_sensor_` (`Pose3`) transform between the pose variable's frame (body) and the camera's sensor frame.\n",
"- **Cheirality Handling:** Can be configured to throw an exception or return a large error if the landmark projects behind the camera.\n",
"\n",
"The error is the 2D vector difference:\n",
"$$ \\text{error}(P, L) = \\text{project}(P \\cdot S, L) - z $$\n",
"where $P$ is the pose variable, $L$ is the landmark variable, $S$ is the optional `body_P_sensor` transform, `project` is the camera projection function including calibration, and $z$ is the `measured_` point."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/ProjectionFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Pose3, Point3, Point2, Rot3, Cal3_S2, Values\n",
"# The Python wrapper often creates specific instantiations\n",
"from gtsam import GenericProjectionFactorCal3_S2\n",
"from gtsam import symbol_shorthand\n",
"\n",
"X = symbol_shorthand.X\n",
"L = symbol_shorthand.L"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_header_md"
},
"source": [
"## Creating a GenericProjectionFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_desc_md"
},
"source": [
"Instantiate by providing:\n",
"1. The 2D measurement (`Point2`).\n",
"2. The noise model (typically 2D isotropic).\n",
"3. The key for the pose variable.\n",
"4. The key for the landmark variable.\n",
"5. A `shared_ptr` to the fixed calibration object.\n",
"6. (Optional) The fixed `Pose3` sensor offset `body_P_sensor`.\n",
"7. (Optional) Cheirality handling flags."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Factor with offset: GenericProjectionFactor, z = [\n",
"\t330;\n",
"\t250\n",
"]\n",
" sensor pose in body frame: R: [\n",
"\t6.12323e-17, 6.12323e-17, 1;\n",
"\t-1, 3.7494e-33, 6.12323e-17;\n",
"\t-0, -1, 6.12323e-17\n",
"]\n",
"t: 0.1 0 0.2\n",
" keys = { x0 l1 }\n",
" noise model: unit (2) \n",
"\n",
"Factor without offset: GenericProjectionFactor, z = [\n",
"\t330;\n",
"\t250\n",
"]\n",
" keys = { x0 l1 }\n",
" noise model: unit (2) \n"
]
}
],
"source": [
"measured_pt2 = Point2(330, 250)\n",
"pixel_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # 1 pixel std dev\n",
"pose_key = X(0)\n",
"landmark_key = L(1)\n",
"\n",
"# Shared pointer to calibration\n",
"K = Cal3_S2(500.0, 500.0, 0.0, 320.0, 240.0)\n",
"\n",
"# Optional sensor pose offset\n",
"body_P_sensor = Pose3(Rot3.Ypr(-np.pi/2, 0, -np.pi/2), Point3(0.1, 0, 0.2))\n",
"\n",
"# Create factor with sensor offset\n",
"factor_with_offset = GenericProjectionFactorCal3_S2(\n",
" measured_pt2, pixel_noise, pose_key, landmark_key, K, body_P_sensor)\n",
"factor_with_offset.print(\"Factor with offset: \")\n",
"\n",
"# Create factor without sensor offset (implicitly identity)\n",
"factor_no_offset = GenericProjectionFactorCal3_S2(\n",
" measured_pt2, pixel_noise, pose_key, landmark_key, K)\n",
"factor_no_offset.print(\"\\nFactor without offset: \")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_header_md"
},
"source": [
"## Evaluating the Error"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_desc_md"
},
"source": [
"The error is the difference between the predicted projection and the measurement."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Error (no offset): 1175689.2145311693\n",
"Error (with offset): 50751.576015003826\n",
"Manual error calculation (no offset): [1370.63962025 687.55033305]\n"
]
}
],
"source": [
"values = Values()\n",
"\n",
"# Example values\n",
"pose = Pose3(Rot3.Rodrigues(0.1, -0.2, 0.3), Point3(1, -1, 0.5))\n",
"landmark = Point3(4.0, 2.0, 3.0)\n",
"\n",
"values.insert(pose_key, pose)\n",
"values.insert(landmark_key, landmark)\n",
"\n",
"# Evaluate factor without offset\n",
"error_no_offset = factor_no_offset.error(values)\n",
"print(f\"Error (no offset): {error_no_offset}\")\n",
"\n",
"# Evaluate factor with offset\n",
"error_with_offset = factor_with_offset.error(values)\n",
"print(f\"Error (with offset): {error_with_offset}\")\n",
"\n",
"# Manually verify projection (no offset case)\n",
"cam_no_offset = gtsam.PinholeCameraCal3_S2(pose, K)\n",
"predicted_no_offset = cam_no_offset.project(landmark)\n",
"manual_error = predicted_no_offset - measured_pt2\n",
"print(f\"Manual error calculation (no offset): {manual_error}\")\n",
"assert np.allclose(factor_no_offset.unwhitenedError(values), manual_error)"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

219
gtsam/slam/doc/ReferenceFrameFactor.ipynb Normal file
View File

@ -0,0 +1,219 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# ReferenceFrameFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`ReferenceFrameFactor<POINT, TRANSFORM>` is a ternary factor used to relate landmark observations made in two different reference frames (e.g., from two different robots or two distinct SLAM sessions).\n",
"It connects:\n",
"1. A landmark (`POINT`) expressed in a 'global' or common reference frame (`globalKey`).\n",
"2. A transform (`TRANSFORM`) representing the pose of a 'local' frame relative to the 'global' frame (`transKey`). Typically `TRANSFORM = global_T_local`.\n",
"3. The *same* landmark (`POINT`) expressed in the 'local' reference frame (`localKey`).\n",
"\n",
"The factor enforces the constraint that the globally expressed landmark, when transformed by the `global_T_local` transform, should match the locally expressed landmark.\n",
"The transformation logic depends on the specific `POINT` and `TRANSFORM` types and is handled by the `transform_point` helper function (which usually calls `Transform::transformFrom`).\n",
"\n",
"Error: $ \\text{Log}(\\text{local}^{-1} \\cdot \\text{transform\\_point}(\\text{trans}, \\text{global})) $\n",
"\n",
"This factor is crucial for multi-robot map merging or combining results from different SLAM sessions where common landmarks have been observed."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/ReferenceFrameFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [
{
"ename": "ImportError",
"evalue": "cannot import name 'ReferenceFrameFactorPoint3Pose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[1], line 5\u001b[0m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Pose3, Point3, Rot3, Values, NonlinearFactorGraph\n\u001b[0;32m 4\u001b[0m \u001b[38;5;66;03m# The Python wrapper creates specific instantiations\u001b[39;00m\n\u001b[1;32m----> 5\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m ReferenceFrameFactorPoint3Pose3\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 8\u001b[0m L \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mL \u001b[38;5;66;03m# Global landmark\u001b[39;00m\n",
"\u001b[1;31mImportError\u001b[0m: cannot import name 'ReferenceFrameFactorPoint3Pose3' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
]
}
],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Pose3, Point3, Rot3, Values, NonlinearFactorGraph\n",
"# The Python wrapper creates specific instantiations\n",
"from gtsam import ReferenceFrameFactorPoint3Pose3\n",
"from gtsam import symbol_shorthand\n",
"\n",
"L = symbol_shorthand.L # Global landmark\n",
"T = symbol_shorthand.T # Transform global_T_local\n",
"l = symbol_shorthand.l # Local landmark"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_header_md"
},
"source": [
"## Creating a ReferenceFrameFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_desc_md"
},
"source": [
"Instantiate by providing the keys for the global landmark, the transform, the local landmark, and a noise model.\n",
"The noise model dimensionality should match the dimension of the `POINT` type."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"ReferenceFrameFactor: : ReferenceFrameFactor(Global: L0, Transform: T0, Local: l0)\n",
" noise model: diagonal sigmas [0.1; 0.1; 0.1];\n"
]
}
],
"source": [
"global_landmark_key = L(0)\n",
"transform_key = T(0)\n",
"local_landmark_key = l(0)\n",
"\n",
"# Noise model on the landmark point difference (e.g., Point3 -> 3 dims)\n",
"noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1) # 10cm std dev\n",
"\n",
"# Factor type includes Point and Transform types\n",
"factor = ReferenceFrameFactorPoint3Pose3(global_landmark_key,\n",
" transform_key,\n",
" local_landmark_key,\n",
" noise)\n",
"factor.print(\"ReferenceFrameFactor: \")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_header_md"
},
"source": [
"## Evaluating the Error"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_desc_md"
},
"source": [
"The error represents how much the transformed global landmark deviates from the local landmark estimate."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Expected local landmark: [ 2. -4. 1.]\n",
"\n",
"Error at ground truth: [-0. 0. 0.] (Should be zero)\n",
"Error with noisy local landmark: [-1. 1. -0.5]\n"
]
}
],
"source": [
"values = Values()\n",
"\n",
"# Example values\n",
"global_landmark = Point3(5.0, 2.0, 1.0)\n",
"global_T_local = Pose3(Rot3.Yaw(np.pi/2), Point3(1, 0, 0))\n",
"expected_local_landmark = global_T_local.transformTo(global_landmark)\n",
"print(f\"Expected local landmark: {expected_local_landmark}\")\n",
"\n",
"values.insert(global_landmark_key, global_landmark)\n",
"values.insert(transform_key, global_T_local)\n",
"values.insert(local_landmark_key, expected_local_landmark)\n",
"\n",
"error_gt = factor.error(values)\n",
"print(f\"\\nError at ground truth: {error_gt} (Should be zero)\")\n",
"\n",
"# Introduce error in the local landmark estimate\n",
"noisy_local_landmark = expected_local_landmark + Point3(0.1, -0.1, 0.05)\n",
"values.update(local_landmark_key, noisy_local_landmark)\n",
"error_noisy = factor.error(values)\n",
"print(f\"Error with noisy local landmark: {error_noisy}\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

229
gtsam/slam/doc/RotateFactor.ipynb Normal file
View File

@ -0,0 +1,229 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# Rotate Factors"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"This header defines factors that constrain an unknown rotation (`Rot3`) based on how it transforms either full rotations or directions.\n",
"\n",
"* `RotateFactor`: Relates two *incremental* rotations measured in different frames using an unknown rotation relating the frames. If $Z = R \\cdot P \\cdot R^T$, where $P = e^{[p]}$ and $Z = e^{[z]}$ are measured incremental rotations (expressed as angular velocity vectors $p$ and $z$), this factor constrains the unknown rotation $R$ such that $p = R z$. The error is $Rz - p$.\n",
"* `RotateDirectionsFactor`: Relates two *directions* (unit vectors, `Unit3`) measured in different frames using an unknown rotation $R$ relating the frames. If $p_{world} = R \\cdot z_{body}$, this factor constrains $R$. The error is the angular difference between the predicted $R z_{body}$ and the measured $p_{world}$."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/RotateFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [
{
"ename": "ImportError",
"evalue": "cannot import name 'RotateFactor' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[1], line 4\u001b[0m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m Rot3, Point3, Unit3, Values\n\u001b[1;32m----> 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m RotateFactor, RotateDirectionsFactor\n\u001b[0;32m 5\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 7\u001b[0m R \u001b[38;5;241m=\u001b[39m symbol_shorthand\u001b[38;5;241m.\u001b[39mR \u001b[38;5;66;03m# For Rotation key\u001b[39;00m\n",
"\u001b[1;31mImportError\u001b[0m: cannot import name 'RotateFactor' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
]
}
],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Rot3, Point3, Unit3, Values\n",
"from gtsam import RotateFactor, RotateDirectionsFactor\n",
"from gtsam import symbol_shorthand\n",
"\n",
"R = symbol_shorthand.R # For Rotation key"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor1_header_md"
},
"source": [
"## 1. `RotateFactor`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor1_desc_md"
},
"source": [
"Constraints an unknown `Rot3` based on corresponding incremental rotation measurements $P$ (predicted/world) and $Z$ (measured/body)."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "factor1_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"RotateFactor: Factor RotateFactor on R0\n",
"Noise model: diagonal sigmas [0.001; 0.001; 0.001];\n",
"RotateFactor:]\n",
"p: [0.01 0.02 0.03]\n",
"z: [ 0.03 -0.01 0.02]\n",
"\n",
"Error at ground truth R: [-0. -0. 0.]\n",
"Error at noisy R: [ 0.039987 0.00999887 -0.00998887]\n"
]
}
],
"source": [
"# Assume P and Z are small incremental rotations\n",
"P = Rot3.Expmap(np.array([0.01, 0.02, 0.03]))\n",
"Z = Rot3.Expmap(np.array([0.03, -0.01, 0.02]))\n",
"rot_key = R(0)\n",
"noise1 = gtsam.noiseModel.Isotropic.Sigma(3, 0.001)\n",
"\n",
"factor1 = RotateFactor(rot_key, P, Z, noise1)\n",
"factor1.print(\"RotateFactor: \")\n",
"\n",
"# Evaluate error\n",
"values = Values()\n",
"# Ground truth R would satisfy P = R*Z\n",
"# R = P * Z.inverse()\n",
"gt_R = P * (Z.inverse())\n",
"values.insert(rot_key, gt_R)\n",
"error1_gt = factor1.error(values)\n",
"print(f\"\\nError at ground truth R: {error1_gt}\")\n",
"\n",
"noisy_R = gt_R * Rot3.Expmap(np.array([0.001, -0.001, 0.001]))\n",
"values.update(rot_key, noisy_R)\n",
"error1_noisy = factor1.error(values)\n",
"print(f\"Error at noisy R: {error1_noisy}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor2_header_md"
},
"source": [
"## 2. `RotateDirectionsFactor`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "factor2_desc_md"
},
"source": [
"Constraints an unknown `Rot3` based on corresponding direction measurements $p_{world}$ (predicted/world) and $z_{body}$ (measured/body)."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "factor2_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"RotateDirectionsFactor: Factor RotateDirectionsFactor on R0\n",
"Noise model: diagonal sigmas [0.01; 0.01];\n",
"RotateDirectionsFactor:\n",
"pUnit3: (1, 0, 0)\n",
"zUnit3: (0, 1, 0)\n",
"\n",
"Check: gt_R * z_body = [ 1.0000000e+00 -6.1232340e-17 0.0000000e+00]\n",
"Error at ground truth R: [-0. 0.]\n",
"Error at noisy R: [-0.99995 -0.99995]\n"
]
}
],
"source": [
"p_world = Unit3(Point3(1, 0, 0)) # Direction in world frame\n",
"z_body = Unit3(Point3(0, 1, 0)) # Corresponding direction in body frame\n",
"noise2 = gtsam.noiseModel.Isotropic.Sigma(2, 0.01) # Noise on 2D tangent space\n",
"\n",
"factor2 = RotateDirectionsFactor(rot_key, p_world, z_body, noise2)\n",
"factor2.print(\"RotateDirectionsFactor: \")\n",
"\n",
"# Ground truth R rotates z_body (0,1,0) to p_world (1,0,0)\n",
"# This corresponds to a -90 deg yaw\n",
"gt_R_dir = Rot3.Yaw(-np.pi/2)\n",
"print(f\"\\nCheck: gt_R * z_body = {gt_R_dir * z_body.point3()}\")\n",
"\n",
"# Evaluate error\n",
"values_dir = Values()\n",
"values_dir.insert(rot_key, gt_R_dir)\n",
"error2_gt = factor2.error(values_dir)\n",
"print(f\"Error at ground truth R: {error2_gt}\")\n",
"\n",
"noisy_R_dir = gt_R_dir * Rot3.Expmap(np.array([0.01, 0, 0.01]))\n",
"values_dir.update(rot_key, noisy_R_dir)\n",
"error2_noisy = factor2.error(values_dir)\n",
"print(f\"Error at noisy R: {error2_noisy}\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

221
gtsam/slam/doc/SmartFactorParams.ipynb Normal file
View File

@ -0,0 +1,221 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# SmartProjectionParams"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`SmartProjectionParams` is a structure used to configure the behavior of \"smart\" factors like `SmartProjectionFactor`, `SmartProjectionPoseFactor`, and `SmartStereoFactor`.\n",
"These factors implicitly manage the triangulation of a 3D point observed by multiple cameras.\n",
"\n",
"Parameters include:\n",
"- **`linearizationMode`**: Determines the type of linear factor produced when `.linearize()` is called.\n",
" - `HESSIAN` (Default): Creates a `RegularHessianFactor` by marginalizing out the point.\n",
" - `IMPLICIT_SCHUR`: Creates a `RegularImplicitSchurFactor`.\n",
" - `JACOBIAN_Q`: Creates a `JacobianFactorQ`.\n",
" - `JACOBIAN_SVD`: Creates a `JacobianFactorSVD`.\n",
"- **`degeneracyMode`**: How to handle cases where triangulation fails or is ill-conditioned.\n",
" - `IGNORE_DEGENERACY` (Default): Factor might become ill-conditioned.\n",
" - `ZERO_ON_DEGENERACY`: Return a zero-information factor (Hessian/Jacobian) if triangulation fails.\n",
" - `HANDLE_INFINITY`: Treat the point as being at infinity (uses `Unit3` representation).\n",
"- **`triangulation`**: A `gtsam.TriangulationParameters` struct containing parameters for the `triangulateSafe` function:\n",
" - `rankTolerance`: Rank tolerance threshold for SVD during triangulation.\n",
" - `enableEPI`: Use Essential matrix check for epipolar inconsistency (only for Point3 landmarks).\n",
" - `landmarkDistanceThreshold`: If point distance is greater than this, use point-at-infinity.\n",
" - `dynamicOutlierRejectionThreshold`: If non-zero, dynamically rejects measurements based on reprojection error (threshold in sigmas).\n",
"- **`retriangulationThreshold`**: If the change in camera poses between linearizations exceeds this threshold (Frobenius norm difference), the point is re-triangulated.\n",
"- **`throwCheirality` / `verboseCheirality`**: Flags inherited from projection factors to control exception handling when a point is behind a camera."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/SmartFactorParams.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"from gtsam import SmartProjectionParams, LinearizationMode, DegeneracyMode"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_header_md"
},
"source": [
"## Creating and Modifying Params"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Default Parameters:\n"
]
},
{
"ename": "TypeError",
"evalue": "__init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionParams()\n\nInvoked with: kwargs: linearizationMode=<LinearizationMode.JACOBIAN_Q: 2>, degeneracyMode=<DegeneracyMode.ZERO_ON_DEGENERACY: 1>, throwCheirality=False, verboseCheirality=True, retriangulationTh=0.001",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mTypeError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[4], line 7\u001b[0m\n\u001b[0;32m 3\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mDefault Parameters:\u001b[39m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 4\u001b[0m \u001b[38;5;66;03m# default_params.print() # TODO\u001b[39;00m\n\u001b[0;32m 5\u001b[0m \n\u001b[0;32m 6\u001b[0m \u001b[38;5;66;03m# Custom parameters\u001b[39;00m\n\u001b[1;32m----> 7\u001b[0m custom_params \u001b[38;5;241m=\u001b[39m \u001b[43mSmartProjectionParams\u001b[49m\u001b[43m(\u001b[49m\n\u001b[0;32m 8\u001b[0m \u001b[43m \u001b[49m\u001b[43mlinearizationMode\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[43mLinearizationMode\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mJACOBIAN_Q\u001b[49m\u001b[43m,\u001b[49m\n\u001b[0;32m 9\u001b[0m \u001b[43m \u001b[49m\u001b[43mdegeneracyMode\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[43mDegeneracyMode\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mZERO_ON_DEGENERACY\u001b[49m\u001b[43m,\u001b[49m\n\u001b[0;32m 10\u001b[0m \u001b[43m \u001b[49m\u001b[43mthrowCheirality\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;28;43;01mFalse\u001b[39;49;00m\u001b[43m,\u001b[49m\n\u001b[0;32m 11\u001b[0m \u001b[43m \u001b[49m\u001b[43mverboseCheirality\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;28;43;01mTrue\u001b[39;49;00m\u001b[43m,\u001b[49m\n\u001b[0;32m 12\u001b[0m \u001b[43m \u001b[49m\u001b[43mretriangulationTh\u001b[49m\u001b[43m \u001b[49m\u001b[38;5;241;43m=\u001b[39;49m\u001b[43m \u001b[49m\u001b[38;5;241;43m1e-3\u001b[39;49m\n\u001b[0;32m 13\u001b[0m \u001b[43m)\u001b[49m\n\u001b[0;32m 14\u001b[0m \u001b[38;5;66;03m# Modify triangulation parameters directly\u001b[39;00m\n\u001b[0;32m 15\u001b[0m custom_params\u001b[38;5;241m.\u001b[39mtriangulation\u001b[38;5;241m.\u001b[39mrankTolerance \u001b[38;5;241m=\u001b[39m \u001b[38;5;241m1e-8\u001b[39m\n",
"\u001b[1;31mTypeError\u001b[0m: __init__(): incompatible constructor arguments. The following argument types are supported:\n 1. gtsam.gtsam.SmartProjectionParams()\n\nInvoked with: kwargs: linearizationMode=<LinearizationMode.JACOBIAN_Q: 2>, degeneracyMode=<DegeneracyMode.ZERO_ON_DEGENERACY: 1>, throwCheirality=False, verboseCheirality=True, retriangulationTh=0.001"
]
}
],
"source": [
"# Default parameters\n",
"default_params = SmartProjectionParams()\n",
"print(\"Default Parameters:\")\n",
"default_params.print()\n",
"\n",
"# Custom parameters\n",
"custom_params = SmartProjectionParams(\n",
" linearizationMode = LinearizationMode.JACOBIAN_Q,\n",
" degeneracyMode = DegeneracyMode.ZERO_ON_DEGENERACY,\n",
" throwCheirality = False,\n",
" verboseCheirality = True,\n",
" retriangulationTh = 1e-3\n",
")\n",
"# Modify triangulation parameters directly\n",
"custom_params.triangulation.rankTolerance = 1e-8\n",
"custom_params.triangulation.enableEPI = False\n",
"custom_params.triangulation.dynamicOutlierRejectionThreshold = 3.0 # Reject points with reproj error > 3*sigma\n",
"\n",
"print(\"\\nCustom Parameters:\")\n",
"custom_params.print()\n",
"\n",
"# Accessing parameters\n",
"print(f\"\\nAccessing Custom Params:\")\n",
"print(f\" Linearization Mode: {custom_params.getLinearizationMode()}\")\n",
"print(f\" Degeneracy Mode: {custom_params.getDegeneracyMode()}\")\n",
"print(f\" Rank Tolerance: {custom_params.getTriangulationParameters().rankTolerance}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "usage_header_md"
},
"source": [
"## Usage"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "usage_desc_md"
},
"source": [
"These `SmartProjectionParams` objects are passed to the constructors of smart factors, like `SmartProjectionPoseFactor`."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "usage_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Smart Factor created with custom params:\n",
"SmartProjectionPoseFactor, z = \n",
" SmartProjectionFactor\n",
"linearizationMode: 2\n",
"triangulationParameters:\n",
"rankTolerance = 1e-08\n",
"enableEPI = false\n",
"landmarkDistanceThreshold = -1\n",
"dynamicOutlierRejectionThreshold = 3\n",
"\n",
"\n",
"result:\n",
"Degenerate\n",
"\n",
"SmartFactorBase, z = \n",
"Factor\n"
]
}
],
"source": [
"from gtsam import SmartProjectionPoseFactorCal3_S2, Cal3_S2\n",
"\n",
"# Example: Using custom params with a smart factor\n",
"noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
"K = gtsam.make_shared_Cal3_S2(500, 500, 0, 320, 240)\n",
"\n",
"smart_factor = SmartProjectionPoseFactorCal3_S2(noise, K, custom_params)\n",
"print(\"Smart Factor created with custom params:\")\n",
"smart_factor.print()"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

343
gtsam/slam/doc/SmartProjectionFactor.ipynb Normal file
View File

@ -0,0 +1,343 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# SmartProjectionFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`SmartProjectionFactor<CAMERA>` is a \"smart\" factor designed for Structure from Motion (SfM) or visual SLAM problems where **both camera poses and calibration are being optimized**.\n",
"It implicitly represents a 3D point landmark that has been observed by multiple cameras.\n",
"\n",
"Key characteristics:\n",
"- **Implicit Point:** The 3D point is not an explicit variable in the factor graph. Instead, the factor internally triangulates the point based on the current camera estimates whenever needed (e.g., during linearization or error calculation).\n",
"- **Marginalization:** When linearized (e.g., to a Hessian factor), it effectively marginalizes out the 3D point, creating a dense factor connecting only the cameras that observed the point.\n",
"- **`CAMERA` Template:** This template parameter must represent a camera model that includes *both* pose and calibration (e.g., `PinholeCameraCal3_S2`, `PinholeCameraBundler`).\n",
"- **`Values` Requirement:** When using this factor, the `Values` object passed to methods like `error` or `linearize` must contain `CAMERA` objects (not separate `Pose3` and `Calib` objects) associated with the factor's keys.\n",
"- **Configuration:** Its behavior (linearization method, handling of degenerate triangulations) is controlled by `SmartProjectionParams`.\n",
"\n",
"**Use Case:** Suitable for Bundle Adjustment or SfM problems where calibration parameters are unknown or need refinement along with camera poses.\n",
"**Alternative:** If calibration is known and fixed, use `SmartProjectionPoseFactor` for better efficiency."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/SmartProjectionFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [
{
"ename": "ImportError",
"evalue": "cannot import name 'SmartProjectionFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n\u001b[0;32m 4\u001b[0m SmartProjectionParams, SmartProjectionFactorPinholeCameraCal3_S2,\n\u001b[0;32m 5\u001b[0m PinholeCameraCal3_S2, Cal3_S2)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 7\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgraphviz\u001b[39;00m\n",
"\u001b[1;31mImportError\u001b[0m: cannot import name 'SmartProjectionFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
]
}
],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n",
" SmartProjectionParams, SmartProjectionFactorPinholeCameraCal3_S2,\n",
" PinholeCameraCal3_S2, Cal3_S2)\n",
"from gtsam import symbol_shorthand\n",
"import graphviz\n",
"\n",
"C = symbol_shorthand.C # Key for Camera object"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_header_md"
},
"source": [
"## Creating and Adding Measurements"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_desc_md"
},
"source": [
"1. Create the factor with a noise model and parameters.\n",
"2. Add measurements (2D points) and the corresponding camera keys one by one or in batches."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Smart factor involves 3 measurements.\n",
"SmartFactor: SmartProjectionFactor\n",
"linearizationMode: 0\n",
"triangulationParameters:\n",
"rankTolerance = 1e-09\n",
"enableEPI = false\n",
"landmarkDistanceThreshold = -1\n",
"dynamicOutlierRejectionThreshold = -1\n",
"\n",
"\n",
"result:\n",
"Degenerate\n",
"\n",
"SmartFactorBase, z = \n",
"measurement 0, px = \n",
"[150; 505]\n",
"\n",
"noise model = diagonal sigmas [1; 1];\n",
"measurement 1, px = \n",
"[470; 495]\n",
"\n",
"noise model = diagonal sigmas [1; 1];\n",
"measurement 2, px = \n",
"[480; 150]\n",
"\n",
"noise model = diagonal sigmas [1; 1];\n",
"Factor on C0 C1 C2\n"
]
}
],
"source": [
"smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
"smart_params = SmartProjectionParams() # Use default params (HESSIAN, IGNORE_DEGENERACY)\n",
"\n",
"# Factor type includes the Camera type, e.g., SmartProjectionFactorPinholeCameraCal3_S2\n",
"smart_factor = SmartProjectionFactorPinholeCameraCal3_S2(smart_noise, smart_params)\n",
"\n",
"# Add measurements and keys\n",
"smart_factor.add(Point2(150, 505), C(0))\n",
"smart_factor.add(Point2(470, 495), C(1))\n",
"smart_factor.add(Point2(480, 150), C(2))\n",
"\n",
"print(f\"Smart factor involves {smart_factor.size()} measurements.\")\n",
"smart_factor.print(\"SmartFactor: \")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_header_md"
},
"source": [
"## Evaluating the Error"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_desc_md"
},
"source": [
"The `.error(values)` method implicitly triangulates the point based on the `CAMERA` objects in `values` and computes the sum of squared reprojection errors."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Triangulated point result:\n",
"Valid triangulation with point [4.15063506 0.18267612 5.20423168]\n",
"\n",
"Total reprojection error (0.5 * sum(err^2/sigma^2)): 103876.2153\n"
]
}
],
"source": [
"# Create Values containing CAMERA objects\n",
"values = Values()\n",
"K = Cal3_S2(500, 500, 0, 320, 240)\n",
"pose0 = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(-1, 0, 0.5))\n",
"pose1 = Pose3(Rot3.Ypr(0.0, 0.1, 0.1), Point3( 1, 0, 0.5))\n",
"pose2 = Pose3(Rot3.Ypr(-0.1, 0.0, -0.2), Point3( 0, 1, 0.5))\n",
"\n",
"values.insert(C(0), PinholeCameraCal3_S2(pose0, K))\n",
"values.insert(C(1), PinholeCameraCal3_S2(pose1, K))\n",
"values.insert(C(2), PinholeCameraCal3_S2(pose2, K))\n",
"\n",
"# Triangulate first to see the implicit point\n",
"point_result = smart_factor.point(values)\n",
"print(f\"Triangulated point result:\\n{point_result}\")\n",
"\n",
"if point_result.valid():\n",
" # Calculate error\n",
" total_error = smart_factor.error(values)\n",
" print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n",
"else:\n",
" print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "linearize_header_md"
},
"source": [
"## Linearization"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "linearize_desc_md"
},
"source": [
"The `.linearize(values)` method creates a linear factor (e.g., `RegularHessianFactor`) connecting the camera variables, effectively marginalizing the implicit point."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "linearize_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Linearized Factor (showing HessianFactor structure):\n",
"RegularHessianFactor(9): density=100% keys={ C0 C1 C2 }\n",
"Augmented information matrix: (dimensions: 9, 9, 9) : \n",
"{\n",
"\t[ 4.383e+04, 2.596e+04, -1.466e+04, -6.732e+03, 1.357e+04, -4.327e+03, -2.004e+04, -1.143e+03, -3.459e+03; ]\n",
"\t[ 2.596e+04, 6.879e+04, 5.416e+03, 3.589e+03, -1.075e+04, 2.158e+04, 3.107e+04, -8.167e+03, -1.237e+04; ]\n",
"\t[ -1.466e+04, 5.416e+03, 2.336e+04, -1.026e+03, -3.572e+03, -1.124e+04, -1.497e+04, -5.631e+03, 4.149e+03; ]\n",
"\t[ -6.732e+03, 3.589e+03, -1.026e+03, 7.027e+03, 1.434e+03, 3.742e+03, 2.527e+03, 2.454e+03, -6.619e+03; ]\n",
"\t[ 1.357e+04, -1.075e+04, -3.572e+03, 1.434e+03, 1.511e+04, -8.935e+02, -1.484e+04, 3.811e+03, -1.993e+03; ]\n",
"\t[ -4.327e+03, 2.158e+04, -1.124e+04, 3.742e+03, -8.935e+02, 2.587e+04, 2.085e+04, -1.193e+04, -5.818e+03; ]\n",
"\t[ -2.004e+04, 3.107e+04, -1.497e+04, 2.527e+03, -1.484e+04, 2.085e+04, 3.128e+04, -5.349e+03, -6.557e+03; ]\n",
"\t[ -1.143e+03, -8.167e+03, -5.631e+03, 2.454e+03, 3.811e+03, -1.193e+04, -5.349e+03, 1.537e+04, -1.987e+03; ]\n",
"\t[ -3.459e+03, -1.237e+04, 4.149e+03, -6.619e+03, -1.993e+03, -5.818e+03, -6.557e+03, -1.987e+03, 1.043e+04; ]\n",
"}\n",
"Augmented Diagonal Block [0,0]:\n",
"[ 4.383e+04, 2.596e+04, -1.466e+04; ]\n",
"[ 2.596e+04, 6.879e+04, 5.416e+03; ]\n",
"[ -1.466e+04, 5.416e+03, 2.336e+04; ]\n",
"\n",
"Augmented Diagonal Block [1,1]:\n",
"[ 7.027e+03, 1.434e+03, 3.742e+03; ]\n",
"[ 1.434e+03, 1.511e+04, -8.935e+02; ]\n",
"[ 3.742e+03, -8.935e+02, 2.587e+04; ]\n",
"\n",
"Augmented Diagonal Block [2,2]:\n",
"[ 3.128e+04, -5.349e+03, -6.557e+03; ]\n",
"[ -5.349e+03, 1.537e+04, -1.987e+03; ]\n",
"[ -6.557e+03, -1.987e+03, 1.043e+04; ]\n",
"\n",
"Off-Diagonal Block [0,1]:\n",
"[ -6.732e+03, 1.357e+04, -4.327e+03; ]\n",
"[ 3.589e+03, -1.075e+04, 2.158e+04; ]\n",
"[ -1.026e+03, -3.572e+03, -1.124e+04; ]\n",
"\n",
"Off-Diagonal Block [0,2]:\n",
"[ -2.004e+04, -1.143e+03, -3.459e+03; ]\n",
"[ 3.107e+04, -8.167e+03, -1.237e+04; ]\n",
"[ -1.497e+04, -5.631e+03, 4.149e+03; ]\n",
"\n",
"Off-Diagonal Block [1,2]:\n",
"[ 2.527e+03, 2.454e+03, -6.619e+03; ]\n",
"[ -1.484e+04, 3.811e+03, -1.993e+03; ]\n",
"[ 2.085e+04, -1.193e+04, -5.818e+03; ]\n",
"\n",
"Error vector:\n",
"[-1.041e+03; -2.952e+03; 1.543e+03; -6.959e+02; -8.037e+02; 1.174e+03; 1.309e+03; 1.288e+03; 1.095e+03]\n",
"Constant error term: 103876\n"
]
}
],
"source": [
"graph = NonlinearFactorGraph()\n",
"graph.add(smart_factor)\n",
"\n",
"# Linearize (default mode is HESSIAN)\n",
"linear_factor = smart_factor.linearize(values)\n",
"\n",
"if linear_factor:\n",
" print(\"\\nLinearized Factor (showing HessianFactor structure):\")\n",
" # Cast to HessianFactor to print its details\n",
" hessian_factor = gtsam.RegularHessianFactorCal3_S2.Downcast(linear_factor)\n",
" if hessian_factor:\n",
" hessian_factor.print()\n",
" else:\n",
" print(\"Linearized factor is not a HessianFactor (check params.linearizationMode)\")\n",
"else:\n",
" print(\"Linearization failed (likely due to triangulation degeneracy)\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

354
gtsam/slam/doc/SmartProjectionPoseFactor.ipynb Normal file
View File

@ -0,0 +1,354 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# SmartProjectionPoseFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`SmartProjectionPoseFactor<CALIBRATION>` is a \"smart\" factor optimized for visual SLAM problems where the **camera calibration is known and fixed**, and only the camera **poses** are being optimized.\n",
"It inherits from `SmartProjectionFactor` but simplifies the setup by taking a single shared `CALIBRATION` object (e.g., `Cal3_S2`, `Cal3Bundler`) assumed to be used by all cameras observing the landmark.\n",
"\n",
"Key characteristics:\n",
"- **Implicit Point:** Like its base class, the 3D point is not an explicit variable.\n",
"- **Fixed Calibration:** Assumes a single, known calibration `K` for all measurements.\n",
"- **Pose Variables:** The factor connects `Pose3` variables directly.\n",
"- **`Values` Requirement:** Requires `Pose3` objects in the `Values` container for its keys.\n",
"- **Configuration:** Behavior is controlled by `SmartProjectionParams`.\n",
"\n",
"**Use Case:** Ideal for visual SLAM or SfM when camera intrinsics are pre-calibrated and assumed constant.\n",
"**Alternative:** If calibration also needs optimization, use `SmartProjectionFactor`."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/SmartProjectionPoseFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [
{
"ename": "ImportError",
"evalue": "cannot import name 'SmartProjectionPoseFactorCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n\u001b[0;32m 4\u001b[0m SmartProjectionParams, SmartProjectionPoseFactorCal3_S2,\n\u001b[0;32m 5\u001b[0m Cal3_S2)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 7\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgraphviz\u001b[39;00m\n",
"\u001b[1;31mImportError\u001b[0m: cannot import name 'SmartProjectionPoseFactorCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
]
}
],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n",
" SmartProjectionParams, SmartProjectionPoseFactorCal3_S2,\n",
" Cal3_S2)\n",
"from gtsam import symbol_shorthand\n",
"import graphviz\n",
"\n",
"X = symbol_shorthand.X # Key for Pose3 variable"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_header_md"
},
"source": [
"## Creating and Adding Measurements"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_desc_md"
},
"source": [
"1. Create the factor with a noise model, the shared calibration `K`, and parameters.\n",
"2. Add measurements (2D points) and the corresponding pose keys."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Smart factor involves 3 measurements.\n",
"SmartFactorPose: SmartProjectionPoseFactor, z = \n",
" SmartProjectionFactor\n",
"linearizationMode: 0\n",
"triangulationParameters:\n",
"rankTolerance = 1e-09\n",
"enableEPI = false\n",
"landmarkDistanceThreshold = -1\n",
"dynamicOutlierRejectionThreshold = -1\n",
"\n",
"\n",
"result:\n",
"Degenerate\n",
"\n",
"SmartFactorBase, z = \n",
"measurement 0, px = \n",
"[150; 505]\n",
"\n",
"noise model = diagonal sigmas [1; 1];\n",
"measurement 1, px = \n",
"[470; 495]\n",
"\n",
"noise model = diagonal sigmas [1; 1];\n",
"measurement 2, px = \n",
"[480; 150]\n",
"\n",
"noise model = diagonal sigmas [1; 1];\n",
"Factor on x0 x1 x2\n"
]
}
],
"source": [
"smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
"smart_params = SmartProjectionParams() # Use default params\n",
"K = gtsam.make_shared_Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n",
"\n",
"# Factor type includes the Calibration type\n",
"smart_factor = SmartProjectionPoseFactorCal3_S2(smart_noise, K, smart_params)\n",
"\n",
"# Add measurements and keys (Pose keys)\n",
"smart_factor.add(Point2(150, 505), X(0))\n",
"smart_factor.add(Point2(470, 495), X(1))\n",
"smart_factor.add(Point2(480, 150), X(2))\n",
"\n",
"print(f\"Smart factor involves {smart_factor.size()} measurements.\")\n",
"smart_factor.print(\"SmartFactorPose: \")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_header_md"
},
"source": [
"## Evaluating the Error"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_desc_md"
},
"source": [
"The `.error(values)` method uses the `Pose3` objects from `values` and the fixed `K` to triangulate the point and compute the error."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Triangulated point result:\n",
"Valid triangulation with point [4.15063506 0.18267612 5.20423168]\n",
"\n",
"Total reprojection error (0.5 * sum(err^2/sigma^2)): 103876.2153\n"
]
}
],
"source": [
"# Create Values containing Pose3 objects\n",
"values = Values()\n",
"pose0 = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(-1, 0, 0.5))\n",
"pose1 = Pose3(Rot3.Ypr(0.0, 0.1, 0.1), Point3( 1, 0, 0.5))\n",
"pose2 = Pose3(Rot3.Ypr(-0.1, 0.0, -0.2), Point3( 0, 1, 0.5))\n",
"\n",
"values.insert(X(0), pose0)\n",
"values.insert(X(1), pose1)\n",
"values.insert(X(2), pose2)\n",
"\n",
"# Triangulate first to see the implicit point\n",
"point_result = smart_factor.point(values)\n",
"print(f\"Triangulated point result:\\n{point_result}\")\n",
"\n",
"if point_result.valid():\n",
" # Calculate error\n",
" total_error = smart_factor.error(values)\n",
" print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n",
"else:\n",
" print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "linearize_header_md"
},
"source": [
"## Linearization"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "linearize_desc_md"
},
"source": [
"Linearization works similarly to `SmartProjectionFactor`, producing a linear factor (default: Hessian) connecting the `Pose3` variables."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "linearize_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Linearized Factor (showing HessianFactor structure):\n",
"RegularHessianFactor(6): density=100% keys={ x0 x1 x2 }\n",
"Augmented information matrix: (dimensions: 6, 6, 6) : \n",
"{\n",
"\t[ 1900, 4864, -698, -5014, -4686, -552; ]\n",
"\t[ 4864, 1.337e+04, 1473, -9.522e+03, -1.623e+04, 447; ]\n",
"\t[ -698, 1473, 4803, 3085, -3660, -1.118e+04; ]\n",
"\t[ -5014, -9.522e+03, 3085, 1318, 2669, -1843; ]\n",
"\t[ -4686, -1.623e+04, -3660, 2669, 4137, -1067; ]\n",
"\t[ -552, 447, -1.118e+04, -1843, -1067, 4035; ]\n",
"\t[ 1318, 2669, -1843, 768, -2084, 849; ]\n",
"\t[ 2669, 4137, -1067, -2084, 6054, -1412; ]\n",
"\t[ -1843, -1067, 4035, 849, -1412, 4392; ]\n",
"\t[ 6273, -1.411e+04, -2161, 11515, 16951, 996; ]\n",
"\t[ -141, -1903, -1.299e+04, 230, -2148, 7017; ]\n",
"\t[ -104, 5192, -1500, -373, -620, -7830; ]\n",
"\t[ -830, 6917, -785, -395, -1398, -708; ]\n",
"\t[ -969, 3198, -1569, 587, 1292, -3268; ]\n",
"\t[ 268, -1.286e+04, 1865, -122, -2205, 2048; ]\n",
"\t[ -708, -1569, -7830, 23563, 2137, 1132; ]\n",
"\t[ -321, 3767, -302, 2137, 15054, -1172; ]\n",
"\t[ -1196, 5036, 1563, 1132, -1172, 8384; ]\n",
"}\n",
"Augmented Diagonal Block [0,0]:\n",
"[ 1900, 4864, -698; ]\n",
"[ 4864, 1.337e+04, 1473; ]\n",
"[ -698, 1473, 4803; ]\n",
"\n",
"Augmented Diagonal Block [1,1]:\n",
"[ 768, -2084, 849; ]\n",
"[ -2084, 6054, -1412; ]\n",
"[ 849, -1412, 4392; ]\n",
"\n",
"Augmented Diagonal Block [2,2]:\n",
"[ 23563, 2137, 1132; ]\n",
"[ 2137, 15054, -1172; ]\n",
"[ 1132, -1172, 8384; ]\n",
"\n",
"Off-Diagonal Block [0,1]:\n",
"[ -5014, -4686, -552; ]\n",
"[ -9.522e+03, -1.623e+04, 447; ]\n",
"[ 3085, -3660, -1.118e+04; ]\n",
"\n",
"Off-Diagonal Block [0,2]:\n",
"[ 11515, 16951, 996; ]\n",
"[ 230, -2148, 7017; ]\n",
"[ -373, -620, -7830; ]\n",
"\n",
"Off-Diagonal Block [1,2]:\n",
"[ 6273, -830, -969; ]\n",
"[ -1.411e+04, 6917, 3198; ]\n",
"[ -2161, -785, -1569; ]\n",
"\n",
"Error vector:\n",
"[-1.041e+03; -2.952e+03; 1.543e+03; -6.959e+02; -8.037e+02; 1.174e+03; 1.309e+03; 1.288e+03; 1.095e+03]\n",
"Constant error term: 103876\n"
]
}
],
"source": [
"graph = NonlinearFactorGraph()\n",
"graph.add(smart_factor)\n",
"\n",
"# Linearize (default mode is HESSIAN)\n",
"linear_factor = smart_factor.linearize(values)\n",
"\n",
"if linear_factor:\n",
" print(\"\\nLinearized Factor (showing HessianFactor structure):\")\n",
" # Cast to HessianFactor to print its details\n",
" # Note: The factor dimension is Pose3::dimension (6)\n",
" hessian_factor = gtsam.RegularHessianFactorPose3.Downcast(linear_factor)\n",
" if hessian_factor:\n",
" hessian_factor.print()\n",
" else:\n",
" print(\"Linearized factor is not a HessianFactor (check params.linearizationMode)\")\n",
"else:\n",
" print(\"Linearization failed (likely due to triangulation degeneracy)\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

418
gtsam/slam/doc/SmartProjectionRigFactor.ipynb Normal file
View File

@ -0,0 +1,418 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# SmartProjectionRigFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`SmartProjectionRigFactor<CAMERA>` is a generalization of `SmartProjectionPoseFactor` designed for multi-camera systems (rigs).\n",
"Like other smart factors, it implicitly represents a 3D point landmark observed by multiple cameras.\n",
"\n",
"Key differences/features:\n",
"- **Multi-Camera Rig:** Assumes a fixed rig configuration, where multiple cameras (`CAMERA` instances, which include fixed intrinsics and fixed extrinsics *relative to the rig's body frame*) are defined.\n",
"- **Pose Variables:** Connects `Pose3` variables representing the pose of the **rig's body frame** in the world.\n",
"- **Multiple Observations per Pose:** Allows multiple measurements associated with the *same* body pose key, but originating from different cameras within the rig.\n",
"- **Camera Indexing:** Each measurement must be associated with both a body pose key and a `cameraId` (index) specifying which camera in the rig took the measurement.\n",
"- **Fixed Calibration/Extrinsics:** The intrinsics and relative extrinsics of the cameras within the rig are assumed fixed.\n",
"- **`CAMERA` Template:** Can be templated on various camera models (e.g., `PinholeCamera`, `SphericalCamera`), provided they adhere to the expected GTSAM camera concepts.\n",
"- **`Values` Requirement:** Requires `Pose3` objects (representing the body frame) in the `Values` container.\n",
"- **Configuration:** Behavior controlled by `SmartProjectionParams`. **Note:** Currently (as of header comment), only supports `HESSIAN` linearization mode and `ZERO_ON_DEGENERACY` mode.\n",
"\n",
"**Use Case:** Ideal for visual SLAM with a calibrated multi-camera rig (e.g., stereo rig, multi-fisheye system) where only the rig's pose is optimized."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/SmartProjectionRigFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [
{
"ename": "ImportError",
"evalue": "cannot import name 'SmartProjectionRigFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mImportError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[1], line 3\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\n\u001b[0;32m 2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnumpy\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mas\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mnp\u001b[39;00m\n\u001b[1;32m----> 3\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n\u001b[0;32m 4\u001b[0m SmartProjectionParams, SmartProjectionRigFactorPinholeCameraCal3_S2,\n\u001b[0;32m 5\u001b[0m CameraSetPinholeCameraCal3_S2, PinholeCameraCal3_S2, Cal3_S2)\n\u001b[0;32m 6\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgtsam\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;28;01mimport\u001b[39;00m symbol_shorthand\n\u001b[0;32m 7\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m\u001b[38;5;250m \u001b[39m\u001b[38;5;21;01mgraphviz\u001b[39;00m\n",
"\u001b[1;31mImportError\u001b[0m: cannot import name 'SmartProjectionRigFactorPinholeCameraCal3_S2' from 'gtsam' (c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\__init__.py)"
]
}
],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import (Values, Point2, Point3, Pose3, Rot3, NonlinearFactorGraph,\n",
" SmartProjectionParams, SmartProjectionRigFactorPinholeCameraCal3_S2,\n",
" CameraSetPinholeCameraCal3_S2, PinholeCameraCal3_S2, Cal3_S2)\n",
"from gtsam import symbol_shorthand\n",
"import graphviz\n",
"\n",
"X = symbol_shorthand.X # Key for Pose3 variable (Body Pose)"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_header_md"
},
"source": [
"## Creating the Rig and Factor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_desc_md"
},
"source": [
"1. Define the camera rig configuration: Create a `CameraSet` containing the `CAMERA` objects (with fixed intrinsics and rig-relative extrinsics).\n",
"2. Create the `SmartProjectionRigFactor` with noise, the rig, and parameters.\n",
"3. Add measurements, specifying the 2D point, the corresponding **body pose key**, and the **camera ID** within the rig."
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Smart factor involves 4 measurements from 2 unique poses.\n",
"SmartFactorRig: SmartProjectionRigFactor: \n",
" -- Measurement nr 0\n",
"key: x0\n",
"cameraId: 0\n",
"camera in rig:\n",
"Pose3:\n",
"R: [\n",
"\t1, 0, 0;\n",
"\t0, 1, 0;\n",
"\t0, 0, 1\n",
"]\n",
" t: 0.1 0 0\n",
"\n",
"Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n",
"-- Measurement nr 1\n",
"key: x0\n",
"cameraId: 1\n",
"camera in rig:\n",
"Pose3:\n",
"R: [\n",
"\t1, 0, 0;\n",
"\t0, 1, 0;\n",
"\t0, 0, 1\n",
"]\n",
" t: 0.1 -0.1 0\n",
"\n",
"Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n",
"-- Measurement nr 2\n",
"key: x1\n",
"cameraId: 0\n",
"camera in rig:\n",
"Pose3:\n",
"R: [\n",
"\t1, 0, 0;\n",
"\t0, 1, 0;\n",
"\t0, 0, 1\n",
"]\n",
" t: 0.1 0 0\n",
"\n",
"Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n",
"-- Measurement nr 3\n",
"key: x1\n",
"cameraId: 1\n",
"camera in rig:\n",
"Pose3:\n",
"R: [\n",
"\t1, 0, 0;\n",
"\t0, 1, 0;\n",
"\t0, 0, 1\n",
"]\n",
" t: 0.1 -0.1 0\n",
"\n",
"Calibration: fx=500 fy=500 s=0 u0=320 v0=240\n",
"SmartProjectionFactor\n",
"linearizationMode: 0\n",
"triangulationParameters:\n",
"rankTolerance = 1e-09\n",
"enableEPI = false\n",
"landmarkDistanceThreshold = -1\n",
"dynamicOutlierRejectionThreshold = -1\n",
"\n",
"\n",
"result:\n",
"Degenerate\n",
"\n",
"SmartFactorBase, z = \n",
"measurement 0, px = \n",
"[300; 200]\n",
"\n",
"noise model = diagonal sigmas [1; 1];\n",
"measurement 1, px = \n",
"[250; 201]\n",
"\n",
"noise model = diagonal sigmas [1; 1];\n",
"measurement 2, px = \n",
"[310; 210]\n",
"\n",
"noise model = diagonal sigmas [1; 1];\n",
"measurement 3, px = \n",
"[260; 211]\n",
"\n",
"noise model = diagonal sigmas [1; 1];\n",
"Factor on x0 x1\n"
]
}
],
"source": [
"# 1. Define the Camera Rig\n",
"K = Cal3_S2(500, 500, 0, 320, 240)\n",
"# Camera 0: Forward facing, slightly offset\n",
"body_P_cam0 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, 0, 0))\n",
"cam0 = PinholeCameraCal3_S2(body_P_cam0, K)\n",
"# Camera 1: Stereo camera, right of cam0\n",
"body_P_cam1 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, -0.1, 0)) # Baseline 0.1m\n",
"cam1 = PinholeCameraCal3_S2(body_P_cam1, K)\n",
"\n",
"rig_cameras = CameraSetPinholeCameraCal3_S2()\n",
"rig_cameras.append(cam0)\n",
"rig_cameras.append(cam1)\n",
"rig_cameras_ptr = gtsam.make_shared_CameraSetPinholeCameraCal3_S2(rig_cameras)\n",
"\n",
"# 2. Create the Factor\n",
"smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
"# Ensure parameters are compatible (HESSIAN, ZERO_ON_DEGENERACY)\n",
"smart_params = SmartProjectionParams(linearizationMode=gtsam.LinearizationMode.HESSIAN,\n",
" degeneracyMode=gtsam.DegeneracyMode.ZERO_ON_DEGENERACY)\n",
"\n",
"# Factor type includes the Camera type\n",
"smart_factor = SmartProjectionRigFactorPinholeCameraCal3_S2(smart_noise, rig_cameras_ptr, smart_params)\n",
"\n",
"# 3. Add measurements\n",
"# Observation from Body Pose X(0), Camera 0\n",
"smart_factor.add(Point2(300, 200), X(0), 0)\n",
"# Observation from Body Pose X(0), Camera 1 (stereo pair?)\n",
"smart_factor.add(Point2(250, 201), X(0), 1)\n",
"# Observation from Body Pose X(1), Camera 0\n",
"smart_factor.add(Point2(310, 210), X(1), 0)\n",
"# Observation from Body Pose X(1), Camera 1\n",
"smart_factor.add(Point2(260, 211), X(1), 1)\n",
"\n",
"print(f\"Smart factor involves {smart_factor.size()} measurements from {len(smart_factor.keys())} unique poses.\")\n",
"smart_factor.print(\"SmartFactorRig: \")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_header_md"
},
"source": [
"## Evaluating the Error"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_desc_md"
},
"source": [
"The `.error(values)` method uses the `Pose3` objects (body poses) from `values` and the fixed rig configuration to triangulate the point and compute the error."
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Triangulated point result:\n",
"Valid triangulation with point [0.70307883 0.20615766 5.18676602]\n",
"\n",
"Total reprojection error (0.5 * sum(err^2/sigma^2)): 181.1904\n"
]
}
],
"source": [
"# Create Values containing Body Pose3 objects\n",
"values = Values()\n",
"pose0 = Pose3(Rot3.Ypr(0.0, 0.0, 0.0), Point3(0, 0, 0))\n",
"pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0))\n",
"values.insert(X(0), pose0)\n",
"values.insert(X(1), pose1)\n",
"\n",
"# Triangulate first to see the implicit point\n",
"# The 'cameras' method internally combines body poses with rig extrinsics\n",
"point_result = smart_factor.point(values)\n",
"print(f\"Triangulated point result:\\n{point_result}\")\n",
"\n",
"if point_result.valid():\n",
" # Calculate error\n",
" total_error = smart_factor.error(values)\n",
" print(f\"\\nTotal reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n",
"else:\n",
" print(\"\\nTriangulation failed, error calculation depends on degeneracyMode.\")\n",
" # Since mode is ZERO_ON_DEGENERACY, error should be 0\n",
" total_error = smart_factor.error(values)\n",
" print(f\"Error when degenerate: {total_error}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "linearize_header_md"
},
"source": [
"## Linearization"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "linearize_desc_md"
},
"source": [
"Linearization (currently restricted to HESSIAN mode) produces a `RegularHessianFactor` connecting the unique body pose (`Pose3`) variables involved."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "linearize_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Linearized Factor (HessianFactor structure):\n",
"RegularHessianFactor(6): density=100% keys={ x0 x1 }\n",
"Augmented information matrix: (dimensions: 6, 6) : \n",
"{\n",
"\t[ 1.257, 8.427, 2.81, -15.09, 3.829, -3.448; ]\n",
"\t[ 8.427, 56.73, 18.91, -101.6, 25.77, -23.21; ]\n",
"\t[ 2.81, 18.91, 6.302, -33.87, 8.589, -7.737; ]\n",
"\t[ -15.09, -101.6, -33.87, 181.2, -46.13, 41.54; ]\n",
"\t[ 3.829, 25.77, 8.589, -46.13, 11.71, -10.54; ]\n",
"\t[ -3.448, -23.21, -7.737, 41.54, -10.54, 9.497; ]\n",
"\t[ 1.257, 8.427, 2.81, -1.257, -8.427, -2.81; ]\n",
"\t[ 8.427, 56.73, 18.91, -8.427, -56.73, -18.91; ]\n",
"\t[ 2.81, 18.91, 6.302, -2.81, -18.91, -6.302; ]\n",
"\t[ -15.09, -101.6, -33.87, 15.09, 101.6, 33.87; ]\n",
"\t[ 3.829, 25.77, 8.589, -3.829, -25.77, -8.589; ]\n",
"\t[ -3.448, -23.21, -7.737, 3.448, 23.21, 7.737; ]\n",
"\t[ 1.257, 8.427, 2.81, -15.09, 3.829, -3.448; ]\n",
"\t[ 8.427, 56.73, 18.91, -101.6, 25.77, -23.21; ]\n",
"\t[ 2.81, 18.91, 6.302, -33.87, 8.589, -7.737; ]\n",
"\t[ 15.09, 101.6, 33.87, -181.2, 46.13, -41.54; ]\n",
"\t[ -3.829, -25.77, -8.589, 46.13, -11.71, 10.54; ]\n",
"\t[ 3.448, 23.21, 7.737, -41.54, 10.54, -9.497; ]\n",
"}\n",
"Augmented Diagonal Block [0,0]:\n",
"[ 1.257, 8.427, 2.81; ]\n",
"[ 8.427, 56.73, 18.91; ]\n",
"[ 2.81, 18.91, 6.302; ]\n",
"\n",
"Augmented Diagonal Block [1,1]:\n",
"[ 1.257, 8.427, 2.81; ]\n",
"[ 8.427, 56.73, 18.91; ]\n",
"[ 2.81, 18.91, 6.302; ]\n",
"\n",
"Off-Diagonal Block [0,1]:\n",
"[ -1.257, -8.427, -2.81; ]\n",
"[ -8.427, -56.73, -18.91; ]\n",
"[ -2.81, -18.91, -6.302; ]\n",
"\n",
"Error vector:\n",
"[-15.087; -101.588; -33.8672; 181.19; -46.1294; 41.5391; 15.087; 101.588; 33.8672; -181.19; 46.1294; -41.5391]\n",
"Constant error term: 181.19\n"
]
}
],
"source": [
"graph = NonlinearFactorGraph()\n",
"graph.add(smart_factor)\n",
"\n",
"# Linearize (HESSIAN mode)\n",
"linear_factor = smart_factor.linearize(values)\n",
"\n",
"if linear_factor:\n",
" print(\"\\nLinearized Factor (HessianFactor structure):\")\n",
" hessian_factor = gtsam.RegularHessianFactorPose3.Downcast(linear_factor)\n",
" if hessian_factor:\n",
" hessian_factor.print()\n",
" else:\n",
" print(\"Linearized factor is not a HessianFactor\")\n",
"else:\n",
" print(\"Linearization failed (likely due to triangulation degeneracy)\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

246
gtsam/slam/doc/StereoFactor.ipynb Normal file
View File

@ -0,0 +1,246 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# GenericStereoFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`GenericStereoFactor<POSE, LANDMARK>` is a factor for handling measurements from a **calibrated stereo camera**.\n",
"It relates a 3D `LANDMARK` (usually `Point3`) to a `StereoPoint2` measurement observed by a stereo camera system defined by a `POSE` (usually `Pose3`) and a fixed stereo calibration `Cal3_S2Stereo`.\n",
"\n",
"`StereoPoint2` contains $(u_L, u_R, v)$, the horizontal pixel coordinates in the left ($u_L$) and right ($u_R$) images, and the vertical pixel coordinate ($v$), which is assumed the same for both images in a rectified stereo setup.\n",
"`Cal3_S2Stereo` holds the intrinsic parameters (focal length, principal point) common to both cameras and the stereo baseline (distance between camera centers).\n",
"\n",
"Key features:\n",
"- **Templated:** Works with different pose and landmark types.\n",
"- **Fixed Calibration:** Assumes the `Cal3_S2Stereo` object (`K_`) is known and fixed.\n",
"- **Sensor Offset:** Optionally handles a fixed `body_P_sensor_` (`Pose3`) transform.\n",
"- **Cheirality Handling:** Can be configured for points behind the camera.\n",
"\n",
"The error is the 3D vector difference:\n",
"$$ \\text{error}(P, L) = \\text{projectStereo}(P \\cdot S, L) - z $$\n",
"where `projectStereo` uses the `StereoCamera` model, $P$ is the pose, $L$ the landmark, $S$ the optional offset, and $z$ is the `measured_` `StereoPoint2`."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/StereoFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Pose3, Point3, StereoPoint2, Rot3, Cal3_S2Stereo, Values\n",
"# The Python wrapper often creates specific instantiations\n",
"from gtsam import GenericStereoFactor3D\n",
"from gtsam import symbol_shorthand\n",
"\n",
"X = symbol_shorthand.X\n",
"L = symbol_shorthand.L"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_header_md"
},
"source": [
"## Creating a GenericStereoFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_desc_md"
},
"source": [
"Instantiate by providing:\n",
"1. The measurement (`StereoPoint2`).\n",
"2. The noise model (typically 3D).\n",
"3. The key for the pose variable.\n",
"4. The key for the landmark variable.\n",
"5. A `shared_ptr` to the fixed stereo calibration object (`Cal3_S2Stereo`).\n",
"6. (Optional) The fixed `Pose3` sensor offset `body_P_sensor`.\n",
"7. (Optional) Cheirality handling flags."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Factor with offset: keys = { x0 l1 }\n",
" noise model: unit (3) \n",
"Factor with offset: .z(330, 305, 250)\n",
" sensor pose in body frame: R: [\n",
"\t6.12323e-17, 6.12323e-17, 1;\n",
"\t-1, 3.7494e-33, 6.12323e-17;\n",
"\t-0, -1, 6.12323e-17\n",
"]\n",
"t: 0.1 0 0.2\n",
"\n",
"Factor without offset: keys = { x0 l1 }\n",
" noise model: unit (3) \n",
"\n",
"Factor without offset: .z(330, 305, 250)\n"
]
}
],
"source": [
"measured_stereo = StereoPoint2(330, 305, 250) # uL, uR, v\n",
"stereo_noise = gtsam.noiseModel.Isotropic.Sigma(3, 1.0) # 1 pixel std dev (ul, ur, v)\n",
"pose_key = X(0)\n",
"landmark_key = L(1)\n",
"\n",
"# Shared pointer to stereo calibration\n",
"K_stereo = Cal3_S2Stereo(500.0, 500.0, 0.0, 320.0, 240.0, 0.1) # fx, fy, s, u0, v0, baseline\n",
"\n",
"# Optional sensor pose offset\n",
"body_P_sensor = Pose3(Rot3.Ypr(-np.pi/2, 0, -np.pi/2), Point3(0.1, 0, 0.2))\n",
"\n",
"# Create factor with sensor offset\n",
"factor_with_offset = GenericStereoFactor3D(\n",
" measured_stereo, stereo_noise, pose_key, landmark_key, K_stereo, body_P_sensor=body_P_sensor)\n",
"factor_with_offset.print(\"Factor with offset: \")\n",
"\n",
"# Create factor without sensor offset\n",
"factor_no_offset = GenericStereoFactor3D(\n",
" measured_stereo, stereo_noise, pose_key, landmark_key, K_stereo)\n",
"factor_no_offset.print(\"\\nFactor without offset: \")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_header_md"
},
"source": [
"## Evaluating the Error"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_desc_md"
},
"source": [
"The error is the 3D difference between the predicted stereo projection and the measurement."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"ename": "AttributeError",
"evalue": "'gtsam.gtsam.Cal3_S2Stereo' object has no attribute 'backproject'",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mAttributeError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[4], line 7\u001b[0m\n\u001b[0;32m 4\u001b[0m pose \u001b[38;5;241m=\u001b[39m Pose3(Rot3\u001b[38;5;241m.\u001b[39mRodrigues(\u001b[38;5;241m0.1\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m0.2\u001b[39m, \u001b[38;5;241m0.3\u001b[39m), Point3(\u001b[38;5;241m1\u001b[39m, \u001b[38;5;241m-\u001b[39m\u001b[38;5;241m1\u001b[39m, \u001b[38;5;241m0.5\u001b[39m))\n\u001b[0;32m 5\u001b[0m \u001b[38;5;66;03m# Triangulate a point that *should* project to measured_stereo\u001b[39;00m\n\u001b[0;32m 6\u001b[0m \u001b[38;5;66;03m# Depth = fx * b / disparity = 500 * 0.1 / (330 - 305) = 50 / 25 = 2.0\u001b[39;00m\n\u001b[1;32m----> 7\u001b[0m expected_point_camera \u001b[38;5;241m=\u001b[39m \u001b[43mK_stereo\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43mbackproject\u001b[49m(measured_stereo)\n\u001b[0;32m 8\u001b[0m landmark \u001b[38;5;241m=\u001b[39m pose\u001b[38;5;241m.\u001b[39mtransformFrom(expected_point_camera)\n\u001b[0;32m 9\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124mf\u001b[39m\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mExpected landmark point: \u001b[39m\u001b[38;5;132;01m{\u001b[39;00mlandmark\u001b[38;5;132;01m}\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n",
"\u001b[1;31mAttributeError\u001b[0m: 'gtsam.gtsam.Cal3_S2Stereo' object has no attribute 'backproject'"
]
}
],
"source": [
"values = Values()\n",
"\n",
"# Example values\n",
"pose = Pose3(Rot3.Rodrigues(0.1, -0.2, 0.3), Point3(1, -1, 0.5))\n",
"# Triangulate a point that *should* project to measured_stereo\n",
"# Depth = fx * b / disparity = 500 * 0.1 / (330 - 305) = 50 / 25 = 2.0\n",
"expected_point_camera = K_stereo.backproject(measured_stereo)\n",
"landmark = pose.transformFrom(expected_point_camera)\n",
"print(f\"Expected landmark point: {landmark}\")\n",
"\n",
"values.insert(pose_key, pose)\n",
"values.insert(landmark_key, landmark)\n",
"\n",
"# Evaluate factor without offset\n",
"error_no_offset = factor_no_offset.error(values)\n",
"print(f\"\\nError (no offset) at expected landmark: {error_no_offset} (Should be zero)\")\n",
"\n",
"# Evaluate factor with offset\n",
"# Need to recompute landmark based on offset pose\n",
"pose_with_offset = pose * body_P_sensor # This is world_P_sensor\n",
"expected_point_offset_cam = K_stereo.backproject(measured_stereo)\n",
"landmark_offset = pose_with_offset.transformFrom(expected_point_offset_cam)\n",
"values.update(landmark_key, landmark_offset)\n",
"error_with_offset = factor_with_offset.error(values)\n",
"print(f\"Error (with offset) at recomputed landmark: {error_with_offset} (Should be zero)\")\n",
"\n",
"# Evaluate with noisy landmark\n",
"noisy_landmark = landmark + Point3(0.1, -0.05, 0.1)\n",
"values.update(landmark_key, noisy_landmark)\n",
"error_no_offset_noisy = factor_no_offset.error(values)\n",
"print(f\"Error (no offset) at noisy landmark: {error_no_offset_noisy}\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

283
gtsam/slam/doc/TriangulationFactor.ipynb Normal file
View File

@ -0,0 +1,283 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# TriangulationFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"`TriangulationFactor<CAMERA>` is a unary factor that constrains a single unknown 3D point (`Point3`) based on a 2D measurement from a **known** camera.\n",
"It's essentially the inverse of a projection factor where the camera pose and calibration are fixed, and only the 3D point is variable.\n",
"\n",
"Key characteristics:\n",
"- **Unary Factor:** Connects only to a `Point3` variable key.\n",
"- **Known Camera:** The `CAMERA` object (e.g., `PinholeCameraCal3_S2`, `StereoCamera`) containing the fixed pose and calibration is provided during factor construction.\n",
"- **Measurement:** Takes a 2D measurement (`Point2` for monocular, `StereoPoint2` for stereo).\n",
"- **Error:** Calculates the reprojection error: $ \\text{error}(L) = \\text{camera.project}(L) - z $\n",
"\n",
"**Use Case:** Useful in triangulation scenarios where multiple camera views with known poses observe an unknown landmark. A `NonlinearFactorGraph` containing only `TriangulationFactor`s (one for each view) can be optimized to find the maximum likelihood estimate of the 3D landmark position."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/TriangulationFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Point3, Point2, Pose3, Rot3, Cal3_S2, PinholeCameraCal3_S2, Values, NonlinearFactorGraph\n",
"# The Python wrapper often creates specific instantiations\n",
"from gtsam import TriangulationFactorCal3_S2\n",
"from gtsam import symbol_shorthand\n",
"\n",
"L = symbol_shorthand.L"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_header_md"
},
"source": [
"## Creating a TriangulationFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_desc_md"
},
"source": [
"Instantiate by providing:\n",
"1. The known `CAMERA` object.\n",
"2. The 2D measurement.\n",
"3. The noise model for the measurement.\n",
"4. The key for the unknown `Point3` landmark.\n",
"5. (Optional) Cheirality handling flags."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"TriangulationFactor: TriangulationFactor,camera.pose R: [\n",
"\t0.990033, -0.117578, -0.0775207;\n",
"\t0.0993347, 0.97319, -0.207445;\n",
"\t0.0998334, 0.197677, 0.97517\n",
"]\n",
"t: 1 0 0.5\n",
"camera.calibration[\n",
"\t500, 0, 320;\n",
"\t0, 500, 240;\n",
"\t0, 0, 1\n",
"]\n",
"z[\n",
"\t450.5;\n",
"\t300.2\n",
"]\n",
" keys = { l0 }\n",
" noise model: unit (2) \n"
]
}
],
"source": [
"# Known camera parameters\n",
"K = Cal3_S2(500.0, 500.0, 0.0, 320.0, 240.0)\n",
"pose = Pose3(Rot3.Ypr(0.1, -0.1, 0.2), Point3(1, 0, 0.5))\n",
"camera = PinholeCameraCal3_S2(pose, K)\n",
"\n",
"# Measurement observed by this camera\n",
"measured_pt2 = Point2(450.5, 300.2)\n",
"pixel_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
"landmark_key = L(0)\n",
"\n",
"# Factor type includes the Camera type\n",
"factor = TriangulationFactorCal3_S2(camera, measured_pt2, pixel_noise, landmark_key)\n",
"factor.print(\"TriangulationFactor: \")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_header_md"
},
"source": [
"## Evaluating the Error"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "eval_desc_md"
},
"source": [
"The error is the reprojection error given an estimate of the landmark's `Point3` position."
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Reprojection error for estimate [3. 0.5 2. ]: 314012.75623020524\n",
"Expected projection: [1225.10768109 467.55726116]\n",
"Manual error calculation: [774.60768109 167.35726116]\n"
]
}
],
"source": [
"values = Values()\n",
"\n",
"# Estimate for the landmark position\n",
"landmark_estimate = Point3(3.0, 0.5, 2.0)\n",
"values.insert(landmark_key, landmark_estimate)\n",
"\n",
"error = factor.error(values)\n",
"print(f\"Reprojection error for estimate {landmark_estimate}: {error}\")\n",
"\n",
"# Calculate expected projection\n",
"expected_projection = camera.project(landmark_estimate)\n",
"manual_error = expected_projection - measured_pt2\n",
"print(f\"Expected projection: {expected_projection}\")\n",
"print(f\"Manual error calculation: {manual_error}\")\n",
"assert np.allclose(factor.unwhitenedError(values), manual_error)"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "usage_header_md"
},
"source": [
"## Usage in Triangulation"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "usage_desc_md"
},
"source": [
"Multiple `TriangulationFactor`s, one for each known camera view, can be added to a graph to solve for the landmark position."
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {
"id": "usage_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Optimized Landmark Position:\n",
"Values with 1 values:\n",
"Value l0: (class Eigen::Matrix<double,-1,1,0,-1,1>)\n",
"[\n",
"\t-12446.1;\n",
"\t-55075.8;\n",
"\t2.39319e+06\n",
"]\n",
"\n",
"Final Error: 7855.8598\n"
]
}
],
"source": [
"# Create a second camera and measurement\n",
"pose2 = Pose3(Rot3.Ypr(-0.1, 0.1, -0.1), Point3(-1, 0, 0.5))\n",
"camera2 = PinholeCameraCal3_S2(pose2, K)\n",
"measured_pt2_cam2 = Point2(180.0, 190.0)\n",
"factor2 = TriangulationFactorCal3_S2(camera2, measured_pt2_cam2, pixel_noise, landmark_key)\n",
"\n",
"# Create graph and add factors\n",
"triangulation_graph = NonlinearFactorGraph()\n",
"triangulation_graph.add(factor)\n",
"triangulation_graph.add(factor2)\n",
"\n",
"# Optimize (requires an initial estimate)\n",
"initial_estimate = Values()\n",
"initial_estimate.insert(landmark_key, Point3(2, 0, 5)) # Initial guess\n",
"\n",
"optimizer = gtsam.LevenbergMarquardtOptimizer(triangulation_graph, initial_estimate)\n",
"result = optimizer.optimize()\n",
"\n",
"print(\"\\nOptimized Landmark Position:\")\n",
"result.print()\n",
"print(f\"Final Error: {triangulation_graph.error(result):.4f}\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

252
gtsam/slam/doc/dataset.ipynb Normal file
View File

@ -0,0 +1,252 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# Dataset Utilities"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"The `gtsam/slam/dataset.h` header provides utility functions for loading and saving factor graph data, particularly in formats commonly used in the SLAM community like TORO ('.graph') and g2o ('.g2o').\n",
"\n",
"Key functions include:\n",
"* `findExampleDataFile(name)`: Locates example dataset files distributed with GTSAM.\n",
"* `load2D(filename, ...)`: Loads a 2D pose graph (Poses and BetweenFactors) from TORO or g2o format.\n",
"* `load3D(filename)`: Loads a 3D pose graph (currently simpler than `load2D`, assumes specific format).\n",
"* `readG2o(filename, is3D)`: A more general function to read g2o files containing various factor and variable types (2D/3D poses, landmarks, measurements).\n",
"* `writeG2o(graph, initialEstimate, filename)`: Saves a factor graph and values to the g2o format.\n",
"* `parseVariables<T>`/`parseMeasurements<T>`/`parseFactors<T>`: Lower-level parsing functions (less commonly used directly)."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/dataset.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import gtsam.utils.plot as gtsam_plot\n",
"import matplotlib.pyplot as plt\n",
"import numpy as np\n",
"import os\n",
"\n",
"# Ensure the example datasets are available\n",
"# In Colab/pip install, they are usually included.\n",
"# If running locally from build, findExampleDataFile works.\n",
"# If running locally from install without examples, this might fail.\n",
"try:\n",
" # Check for a common example file\n",
" gtsam.findExampleDataFile(\"w100.graph\")\n",
" HAVE_EXAMPLES = True\n",
"except RuntimeError:\n",
" print(\"Warning: Example datasets not found.\")\n",
" print(\"Try running from build directory or installing examples.\")\n",
" HAVE_EXAMPLES = False\n",
"\n",
"# Create dummy files for writing examples if needed\n",
"if not os.path.exists(\"output\"):\n",
" os.makedirs(\"output\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "find_header_md"
},
"source": [
"## Finding Example Datasets"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "find_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Found w100.graph at: c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\Data\\w100.graph\n",
"Found dlr.g2o at: None\n"
]
}
],
"source": [
"if HAVE_EXAMPLES:\n",
" try:\n",
" w100_path = gtsam.findExampleDataFile(\"w100.graph\")\n",
" print(f\"Found w100.graph at: {w100_path}\")\n",
" dlr_path = gtsam.findExampleDataFile(\"dlr.g2o\")\n",
" print(f\"Found dlr.g2o at: {dlr_path}\")\n",
" except RuntimeError as e:\n",
" print(f\"Error finding example file: {e}\")\n",
"else:\n",
" print(\"Skipping findExampleDataFile test as examples are not available.\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "load2d_header_md"
},
"source": [
"## Loading 2D Datasets (`load2D`)"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {
"id": "load2d_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Loaded c:\\Users\\porte\\miniconda3\\envs\\gtsam\\Lib\\site-packages\\gtsam\\Data\\w100.graph:\n",
" Graph size: 300\n",
" Initial estimate keys: 100\n"
]
}
],
"source": [
"if HAVE_EXAMPLES:\n",
" # Load TORO 2D file\n",
" graph_2d, initial_2d = gtsam.load2D(w100_path)\n",
" print(f\"\\nLoaded {w100_path}:\")\n",
" print(f\" Graph size: {graph_2d.size()}\")\n",
" print(f\" Initial estimate keys: {len(initial_2d.keys())}\")\n",
"\n",
" # Plot initial estimate (optional)\n",
" for key in initial_2d.keys():\n",
" gtsam_plot.plot_pose2(0, initial_2d.atPose2(key))\n",
" plt.title(\"Initial Estimate from w100.graph\")\n",
" plt.axis('equal')\n",
" # plt.show() # Uncomment to display plot\n",
" plt.close() # Close plot to prevent display in output\n",
"else:\n",
" print(\"\\nSkipping load2D test.\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "g2o_header_md"
},
"source": [
"## Loading/Saving G2O Files (`readG2o`, `writeG2o`)"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "g2o_desc_md"
},
"source": [
"`readG2o` can handle both 2D and 3D datasets and various factor types defined in the g2o format.\n",
"`writeG2o` saves a `NonlinearFactorGraph` and `Values` into a g2o file."
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {
"id": "g2o_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Error processing None: readG2o(): incompatible function arguments. The following argument types are supported:\n",
" 1. (filename: str, is3D: bool = False, kernelFunctionType: gtsam.gtsam.KernelFunctionType = <KernelFunctionType.KernelFunctionTypeNONE: 0>) -> tuple[gtsam.gtsam.NonlinearFactorGraph, gtsam.gtsam.Values]\n",
"\n",
"Invoked with: None; kwargs: is3D=True\n"
]
}
],
"source": [
"if HAVE_EXAMPLES:\n",
" # Load a 3D g2o file\n",
" try:\n",
" graph_3d, initial_3d = gtsam.readG2o(dlr_path, is3D=True)\n",
" print(f\"\\nLoaded {dlr_path} (3D):\")\n",
" print(f\" Graph size: {graph_3d.size()}\")\n",
" print(f\" Initial estimate keys: {len(initial_3d.keys())}\")\n",
" # You could optimize graph_3d with initial_3d here...\n",
" print(\"Optimization skipped for brevity.\")\n",
" optimized_values = initial_3d # Use initial for demo write\n",
"\n",
" # Save the graph and values back to a g2o file\n",
" output_g2o_file = os.path.join(\"output\", \"dlr_rewrite.g2o\")\n",
" gtsam.writeG2o(graph_3d, optimized_values, output_g2o_file)\n",
" print(f\" Saved graph and values to {output_g2o_file}\")\n",
" # Clean up dummy file\n",
" # os.remove(output_g2o_file)\n",
" # os.rmdir(\"output\")\n",
" except (RuntimeError, TypeError) as e:\n",
" print(f\"Error processing {dlr_path}: {e}\")\n",
"else:\n",
" print(\"\\nSkipping readG2o/writeG2o test.\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

242
gtsam/slam/doc/lago.ipynb Normal file
View File

@ -0,0 +1,242 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# LAGO (Linear Approximation for Graph Optimization)"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"The `gtsam::lago` namespace provides functions for initializing `Pose2` estimates in a 2D SLAM factor graph.\n",
"LAGO stands for Linear Approximation for Graph Optimization. It leverages the structure of typical 2D SLAM problems to efficiently compute an initial guess, particularly for the orientations, which are often the most challenging part for nonlinear solvers.\n",
"\n",
"The core idea is:\n",
"1. **Estimate Orientations:** Assume orientations are independent of translations and solve a linear system just for the orientations ($\theta$). This exploits the fact that the orientation part of the `Pose2` `BetweenFactor` error is approximately linear for small errors.\n",
"2. **Estimate Translations:** Given the estimated orientations, compute the translations by solving another linear system.\n",
"\n",
"Key functions:\n",
"* `initializeOrientations(graph)`: Computes initial estimates for the `Rot2` (orientation) components of the poses in the graph.\n",
"* `initialize(graph)`: Computes initial estimates for the full `Pose2` variables (orientations and translations).\n",
"* `initialize(graph, initialGuess)`: Corrects only the orientation part of a given `initialGuess` using LAGO.\n",
"\n",
"LAGO typically assumes the graph contains a spanning tree of odometry measurements and a prior on the starting pose."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "colab_badge_md"
},
"source": [
"<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/lago.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"%pip install --quiet gtsam"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import gtsam.utils.plot as gtsam_plot\n",
"import matplotlib.pyplot as plt\n",
"import numpy as np\n",
"from gtsam import NonlinearFactorGraph, Values, Pose2, Point3, PriorFactorPose2, BetweenFactorPose2\n",
"from gtsam import lago\n",
"from gtsam import symbol_shorthand\n",
"\n",
"X = symbol_shorthand.X"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "example_header_md"
},
"source": [
"## Example Initialization"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "example_desc_md"
},
"source": [
"We'll create a simple 2D pose graph with odometry and a loop closure, then use `lago.initialize`."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "example_pipeline_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Original Factor Graph:\n",
"size: 6\n",
"\n",
"Factor 0: PriorFactor on x0\n",
" prior mean: (0, 0, 0)\n",
" noise model: diagonal sigmas [0.1; 0.1; 0.05];\n",
"\n",
"Factor 1: BetweenFactor(x0,x1)\n",
" measured: (2, 0, 0)\n",
" noise model: diagonal sigmas [0.2; 0.2; 0.1];\n",
"\n",
"Factor 2: BetweenFactor(x1,x2)\n",
" measured: (2, 0, 1.57079633)\n",
" noise model: diagonal sigmas [0.2; 0.2; 0.1];\n",
"\n",
"Factor 3: BetweenFactor(x2,x3)\n",
" measured: (2, 0, 1.57079633)\n",
" noise model: diagonal sigmas [0.2; 0.2; 0.1];\n"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Factor 4: BetweenFactor(x3,x4)\n",
" measured: (2, 0, 1.57079633)\n",
" noise model: diagonal sigmas [0.2; 0.2; 0.1];\n",
"\n",
"Factor 5: BetweenFactor(x4,x0)\n",
" measured: (2.1, 0.1, 1.62079633)\n",
" noise model: diagonal sigmas [0.25; 0.25; 0.15];\n",
"\n"
]
},
{
"ename": "IndexError",
"evalue": "invalid map<K, T> key",
"output_type": "error",
"traceback": [
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
"\u001b[1;31mIndexError\u001b[0m Traceback (most recent call last)",
"Cell \u001b[1;32mIn[3], line 25\u001b[0m\n\u001b[0;32m 22\u001b[0m graph\u001b[38;5;241m.\u001b[39mprint(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;124mOriginal Factor Graph:\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 24\u001b[0m \u001b[38;5;66;03m# 2. Perform LAGO initialization\u001b[39;00m\n\u001b[1;32m---> 25\u001b[0m initial_estimate_lago \u001b[38;5;241m=\u001b[39m \u001b[43mlago\u001b[49m\u001b[38;5;241;43m.\u001b[39;49m\u001b[43minitialize\u001b[49m\u001b[43m(\u001b[49m\u001b[43mgraph\u001b[49m\u001b[43m)\u001b[49m\n\u001b[0;32m 27\u001b[0m \u001b[38;5;28mprint\u001b[39m(\u001b[38;5;124m\"\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124mInitial Estimate from LAGO:\u001b[39m\u001b[38;5;130;01m\\n\u001b[39;00m\u001b[38;5;124m\"\u001b[39m)\n\u001b[0;32m 28\u001b[0m initial_estimate_lago\u001b[38;5;241m.\u001b[39mprint()\n",
"\u001b[1;31mIndexError\u001b[0m: invalid map<K, T> key"
]
}
],
"source": [
"# 1. Create a NonlinearFactorGraph with Pose2 factors\n",
"graph = NonlinearFactorGraph()\n",
"\n",
"# Add a prior on the first pose\n",
"prior_mean = Pose2(0.0, 0.0, 0.0)\n",
"prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))\n",
"graph.add(PriorFactorPose2(X(0), prior_mean, prior_noise))\n",
"\n",
"# Add odometry factors (simulating moving in a square)\n",
"odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))\n",
"graph.add(BetweenFactorPose2(X(0), X(1), Pose2(2.0, 0.0, 0.0), odometry_noise))\n",
"graph.add(BetweenFactorPose2(X(1), X(2), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n",
"graph.add(BetweenFactorPose2(X(2), X(3), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n",
"graph.add(BetweenFactorPose2(X(3), X(4), Pose2(2.0, 0.0, np.pi/2), odometry_noise))\n",
"\n",
"# Add a loop closure factor\n",
"loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25, 0.15]))\n",
"# Ideal loop closure would be Pose2(2.0, 0.0, np.pi/2)\n",
"measured_loop = Pose2(2.1, 0.1, np.pi/2 + 0.05)\n",
"graph.add(BetweenFactorPose2(X(4), X(0), measured_loop, loop_noise))\n",
"\n",
"graph.print(\"Original Factor Graph:\\n\")\n",
"\n",
"# 2. Perform LAGO initialization\n",
"initial_estimate_lago = lago.initialize(graph)\n",
"\n",
"print(\"\\nInitial Estimate from LAGO:\\n\")\n",
"initial_estimate_lago.print()\n",
"\n",
"# 3. Visualize the LAGO estimate (optional)\n",
"plt.figure(1)\n",
"for key in initial_estimate_lago.keys():\n",
" gtsam_plot.plot_pose2(1, initial_estimate_lago.atPose2(key), 0.5)\n",
"plt.title(\"LAGO Initial Estimate\")\n",
"plt.axis('equal')\n",
"# plt.show()\n",
"plt.close() # Close plot to prevent display in output\n",
"\n",
"# 4. Compare orientation initialization\n",
"initial_orientations = lago.initializeOrientations(graph)\n",
"print(\"\\nLAGO Orientations (VectorValues):\")\n",
"initial_orientations.print()\n",
"\n",
"print(\"Orientations from full LAGO Values:\")\n",
"for i in range(5):\n",
" print(f\" X({i}): {initial_estimate_lago.atPose2(X(i)).theta():.4f}\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "notes_header_md"
},
"source": [
"## Notes"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "notes_desc_md"
},
"source": [
"- LAGO provides a good initial guess, especially for orientations, which can significantly help the convergence of nonlinear optimizers.\n",
"- It assumes the graph structure allows for the orientation estimation (typically requires a spanning tree and a prior).\n",
"- The translation estimates are computed based on the fixed, estimated orientations."
]
}
],
"metadata": {
"kernelspec": {
"display_name": "gtsam",
"language": "python",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.13.1"
}
},
"nbformat": 4,
"nbformat_minor": 0
}