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

Merge pull request #2111 from borglab/new-docs-slam 

New docs: SLAM
release/4.3a0
Frank Dellaert 2025-04-27 17:56:43 -04:00 committed by GitHub
parent beceb654b6 8f5e78c2b5
commit 42cc0ac922
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
37 changed files with 6310 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/geometry/SimpleCamera.h
View File

@ -31,6 +31,11 @@ namespace gtsam {
/// Convenient aliases for Pinhole camera classes with different calibrations.
/// Also needed as forward declarations in the wrapper.
using PinholePoseCal3_S2 = gtsam::PinholePose<gtsam::Cal3_S2>;
using PinholePoseCal3Bundler = gtsam::PinholePose<gtsam::Cal3Bundler>;
using PinholePoseCal3DS2 = gtsam::PinholePose<gtsam::Cal3DS2>;
using PinholePoseCal3Unified = gtsam::PinholePose<gtsam::Cal3Unified>;
using PinholePoseCal3Fisheye = gtsam::PinholePose<gtsam::Cal3Fisheye>;
using PinholeCameraCal3_S2 = gtsam::PinholeCamera<gtsam::Cal3_S2>;
using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;

41
gtsam/geometry/geometry.i
View File

@ -694,6 +694,45 @@ class Unit3 {
bool equals(const gtsam::Unit3& expected, double tol) const;
};
#include <gtsam/geometry/OrientedPlane3.h>
class OrientedPlane3 {
// Standard constructors
OrientedPlane3();
OrientedPlane3(const gtsam::Unit3& n, double d);
OrientedPlane3(const gtsam::Vector& vec);
OrientedPlane3(double a, double b, double c, double d);
// Testable
void print(string s = "") const;
bool equals(const gtsam::OrientedPlane3& s, double tol = 1e-9) const;
gtsam::OrientedPlane3 transform(const gtsam::Pose3& xr) const;
gtsam::OrientedPlane3 transform(const gtsam::Pose3& xr,
Eigen::Ref<Eigen::MatrixXd> Hp,
Eigen::Ref<Eigen::MatrixXd> Hr) const;
gtsam::Vector3 errorVector(const gtsam::OrientedPlane3& other) const;
gtsam::Vector3 errorVector(const gtsam::OrientedPlane3& other,
Eigen::Ref<Eigen::MatrixXd> H1,
Eigen::Ref<Eigen::MatrixXd> H2) const;
static size_t Dim();
size_t dim() const;
gtsam::OrientedPlane3 retract(const gtsam::Vector3& v) const;
gtsam::OrientedPlane3 retract(const gtsam::Vector3& v,
Eigen::Ref<Eigen::MatrixXd> H) const;
gtsam::Vector3 localCoordinates(const gtsam::OrientedPlane3& s) const;
gtsam::Vector planeCoefficients() const;
gtsam::Unit3 normal() const;
gtsam::Unit3 normal(Eigen::Ref<Eigen::MatrixXd> H) const;
double distance() const;
double distance(Eigen::Ref<Eigen::MatrixXd> H) const;
};
#include <gtsam/geometry/EssentialMatrix.h>
class EssentialMatrix {
// Standard Constructors
@ -1278,7 +1317,7 @@ class Similarity3 {
double scale() const;
};
template <T>
template <T = {gtsam::PinholePoseCal3_S2}>
class CameraSet {
CameraSet();

7
gtsam/geometry/triangulation.h
View File

@ -758,10 +758,17 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
}
// Vector of Cameras - used by the Python/MATLAB wrapper
using CameraSetPinholePoseCal3Bundler = CameraSet<PinholePose<Cal3Bundler>>;
using CameraSetPinholePoseCal3_S2 = CameraSet<PinholePose<Cal3_S2>>;
using CameraSetPinholePoseCal3DS2 = CameraSet<PinholePose<Cal3DS2>>;
using CameraSetPinholePoseCal3Fisheye = CameraSet<PinholePose<Cal3Fisheye>>;
using CameraSetPinholePoseCal3Unified = CameraSet<PinholePose<Cal3Unified>>;
using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
using CameraSetCal3DS2 = CameraSet<PinholeCamera<Cal3DS2>>;
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
using CameraSetSpherical = CameraSet<SphericalCamera>;
} // \namespace gtsam

5
gtsam/nonlinear/values.i
View File

@ -12,6 +12,7 @@ namespace gtsam {
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/FundamentalMatrix.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
@ -94,6 +95,7 @@ class Values {
void insert(size_t j, const gtsam::EssentialMatrix& E);
void insert(size_t j, const gtsam::FundamentalMatrix& F);
void insert(size_t j, const gtsam::SimpleFundamentalMatrix& F);
void insert(size_t j, const gtsam::OrientedPlane3& plane);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
void insert(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
@ -138,6 +140,7 @@ class Values {
void update(size_t j, const gtsam::EssentialMatrix& E);
void update(size_t j, const gtsam::FundamentalMatrix& F);
void update(size_t j, const gtsam::SimpleFundamentalMatrix& F);
void update(size_t j, const gtsam::OrientedPlane3& plane);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
void update(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
@ -179,6 +182,7 @@ class Values {
void insert_or_assign(size_t j, const gtsam::EssentialMatrix& E);
void insert_or_assign(size_t j, const gtsam::FundamentalMatrix& F);
void insert_or_assign(size_t j, const gtsam::SimpleFundamentalMatrix& F);
void insert_or_assign(size_t j, const gtsam::OrientedPlane3& plane);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3Bundler>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3f>& camera);
void insert_or_assign(size_t j, const gtsam::PinholeCamera<gtsam::Cal3_S2>& camera);
@ -216,6 +220,7 @@ class Values {
gtsam::EssentialMatrix,
gtsam::FundamentalMatrix,
gtsam::SimpleFundamentalMatrix,
gtsam::OrientedPlane3,
gtsam::PinholeCamera<gtsam::Cal3Bundler>,
gtsam::PinholeCamera<gtsam::Cal3f>,
gtsam::PinholeCamera<gtsam::Cal3_S2>,

10
gtsam/slam/KarcherMeanFactor.h
View File

@ -27,13 +27,17 @@
namespace gtsam {
/**
* Optimize for the Karcher mean, minimizing the geodesic distance to each of
* the given rotations, by constructing a factor graph out of simple
* the given Lie groups elements, by constructing a factor graph out of simple
* PriorFactors.
*/
template <class T>
T FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &rotations);
typename std::enable_if<traits<T>::IsLieGroup, T>::type
FindKarcherMean(const std::vector<T, Eigen::aligned_allocator<T>> &elements);
template <class T> T FindKarcherMean(std::initializer_list<T> &&rotations);
/// FindKarcherMean version from initializer list
template <class T>
typename std::enable_if<traits<T>::IsLieGroup, T>::type
FindKarcherMean(std::initializer_list<T> &&elements);
/**
* The KarcherMeanFactor creates a constraint on all SO(n) variables with

2
gtsam/slam/PlanarProjectionFactor.h
View File

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

24
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -46,6 +46,30 @@ namespace gtsam {
* calibration (i.e., are from the same camera), use SmartProjectionPoseFactor
* instead! If the calibration should be optimized, as well, use
* SmartProjectionFactor instead!
*
* <b>Note on Template Parameter `CAMERA`:</b>
* While this factor is templated on `CAMERA` to allow for generality (e.g.,
* `SphericalCamera`), the current internal implementation for linearization
* (specifically, methods like `createHessianFactor` involving Schur complement
* calculations inherited or adapted from base classes) has limitations. It
* implicitly assumes that the CAMERA only has a Pose3 unknown.
*
* Consequently:
* - This factor works correctly with `CAMERA` types where this is the case,
* such as `PinholePose<CALIBRATION>` or `SphericalCamera`.
* - Using `CAMERA` types where `dimension != 6`, such as
* `PinholeCamera<CALIBRATION>` (which has dimension `6 + CalDim`), will lead
* to compilation errors due to matrix dimension mismatches.
*
* Therefore, for standard pinhole cameras within a fixed rig, use
* `PinholePose<CALIBRATION>` as the `CAMERA` template parameter when defining
* the `CameraSet` passed to this factor's constructor.
*
* TODO(dellaert): Refactor the internal linearization logic (e.g., in
* `createHessianFactor`) to explicitly compute Jacobians with respect to the
* 6-DoF body pose by correctly applying the chain rule, rather than relying on
* `traits<CAMERA>::dimension` for downstream calculations.
*
* @ingroup slam
*/
template <class CAMERA>

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-develop"
]
},
{
"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-develop"
]
},
{
"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-develop"
]
},
{
"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
}

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

@ -0,0 +1,257 @@
{
"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-develop"
]
},
{
"cell_type": "code",
"execution_count": 1,
"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": 4,
"metadata": {
"id": "fprior_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"FrobeniusPriorRot3: keys = { r0 }\n",
" noise model: unit (9) \n",
"\n",
"FrobeniusPrior error (vectorized matrix diff): 9.999916666944462e-05\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}\")"
]
},
{
"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": 7,
"metadata": {
"id": "ffactor_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"FrobeniusFactorRot3: keys = { r0 r1 }\n",
" noise model: unit (9) \n",
"\n",
"FrobeniusFactor error (vectorized matrix diff): 2.499994791671017e-05\n"
]
}
],
"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(key1, Rot3.Yaw(0.11))\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}\")"
]
},
{
"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": 9,
"metadata": {
"id": "fbetween_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"FrobeniusBetweenFactorRot3: FrobeniusBetweenFactor<class gtsam::Rot3>(r0,r1)\n",
" T12: [\n",
"\t0.999988, -0.00499998, 0;\n",
"\t0.00499998, 0.999988, 0;\n",
"\t0, 0, 1\n",
"]\n",
" noise model: unit (9) \n",
"\n",
"FrobeniusBetweenFactor error: 1.925929944387236e-34\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}\")"
]
}
],
"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
}

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-develop"
]
},
{
"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-develop"
]
},
{
"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
}

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

@ -0,0 +1,228 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# Karcher Mean Factors"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"The [KarcherMeanFactor.h](https://github.com/borglab/gtsam/blob/develop/gtsam/slam/KarcherMeanFactor.h) header provides functionality related to computing and constraining the Karcher mean (or Fréchet mean) of a set of rotations or other manifold values.\n",
"The Karcher mean $\\bar{R}$ of a set of rotations $\\{R_i\\}$ is the rotation that minimizes the sum of squared geodesic distances on the manifold:\n",
"$$ \\bar{R} = \\arg \\min_R \\sum_i d^2(R, R_i) = \\arg \\min_R \\sum_i || \\text{Log}(R_i^{-1} R) ||^2 $$\n",
"\n",
"Functions/Classes:\n",
"* `FindKarcherMean`: 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": 1,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"try:\n",
" import google.colab\n",
" %pip install --quiet gtsam-develop\n",
"except ImportError:\n",
" pass # Not running on Colab, do nothing"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Rot3, FindKarcherMeanRot3, KarcherMeanFactorRot3, Values\n",
"from gtsam.symbol_shorthand import 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 = FindKarcherMeanRot3(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": 6,
"metadata": {
"id": "factor_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"KarcherMeanFactorRot3: keys = { r0 r1 r2 }\n",
"sqrt(beta): 10.0\n",
"\n",
"Jacobian for R(0):\n",
"[[10. 0. 0.]\n",
" [ 0. 10. 0.]\n",
" [ 0. 0. 10.]]\n",
"Jacobian for R(1):\n",
"[[10. 0. 0.]\n",
" [ 0. 10. 0.]\n",
" [ 0. 0. 10.]]\n",
"Jacobian for R(2):\n",
"[[10. 0. 0.]\n",
" [ 0. 10. 0.]\n",
" [ 0. 0. 10.]]\n",
"Error vector b:\n",
"[0. 0. 0.]\n"
]
}
],
"source": [
"keys = [R(0), R(1), R(2)]\n",
"beta = 100.0 # Strength of the constraint\n",
"\n",
"k_factor = KarcherMeanFactorRot3(keys, 3, beta)\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",
"\n",
"# Check the Jacobian blocks (should be sqrt(beta)*Identity)\n",
"sqrt_beta = np.sqrt(beta)\n",
"A = linearized_factor.getA()\n",
"assert A.shape == (3, 9), f\"Unexpected shape for A: {A.shape}\"\n",
"A0 = A[:, :3]\n",
"A1 = A[:, 3:6]\n",
"A2 = A[:, 6:9]\n",
"b = linearized_factor.getb()\n",
"\n",
"print(f\"sqrt(beta): {sqrt_beta}\")\n",
"print(f\"\\nJacobian for R(0):\\n{A0}\")\n",
"print(f\"Jacobian for R(1):\\n{A1}\")\n",
"print(f\"Jacobian for R(2):\\n{A2}\")\n",
"print(f\"Error vector b:\\n{b}\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "py312",
"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.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

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

@ -0,0 +1,222 @@
{
"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": 1,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"try:\n",
" import google.colab\n",
" %pip install --quiet gtsam-develop\n",
"except ImportError:\n",
" pass # Not running on Colab, do nothing"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"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.symbol_shorthand import X, P"
]
},
{
"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": 3,
"metadata": {
"id": "factor_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"OrientedPlane3Factor: \n",
"OrientedPlane3Factor Factor (x0, p0)\n",
"Measured Plane : 0 0 1 1\n",
"isotropic dim=3 sigma=0.05\n",
"\n",
"Error at ground truth: 0.0\n",
"\n",
"Error with noisy plane: 0.6469041114912286\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.planeCoefficients()\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 difference between the estimated plane's normal and the measured plane's normal, but as directions only have 2 DOF, the noise model also has to have dimension 2."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {
"id": "prior_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"OrientedPlane3DirectionPrior: \n",
"OrientedPlane3DirectionPrior: Prior Factor on p0\n",
"Measured Plane : 0 0 1 0\n",
"isotropic dim=2 sigma=0.02\n",
"\n",
"Error for prior on noisy_plane: 0.2550239722533919\n",
"Error for prior on gt_plane_world: 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(2, 0.02)\n",
"\n",
"prior_factor = OrientedPlane3DirectionPrior(plane_key, measured_prior_plane.planeCoefficients(), 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": "py312",
"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.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

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

@ -0,0 +1,284 @@
{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"id": "intro_md"
},
"source": [
"# PlanarProjectionFactor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "desc_md"
},
"source": [
"The `PlanarProjectionFactor` variants provide camera projection factors specifically designed for scenarios where **the robot or camera moves primarily on a 2D plane** (e.g., ground robots with cameras).\n",
"They relate a 3D landmark point to a 2D pixel measurement observed by a camera, considering:\n",
"* The robot's 2D pose (`Pose2` `wTb`: body in world frame) in the ground plane.\n",
"* The camera's fixed 3D pose relative to the robot's body frame (`Pose3` `bTc`: body-to-camera).\n",
"* The camera's intrinsic calibration (including distortion, typically `Cal3DS2` or similar).\n",
"* The 3D landmark position in the world frame.\n",
"\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": 1,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"try:\n",
" import google.colab\n",
" %pip install --quiet gtsam-develop\n",
"except ImportError:\n",
" pass # Not running on Colab, do nothing"
]
},
{
"cell_type": "code",
"execution_count": 2,
"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.symbol_shorthand import X, L, C, O"
]
},
{
"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.6472637491106\n"
]
}
],
"source": [
"# Known parameters\n",
"landmark_pt = Point3(2.0, 0.5, 0.5)\n",
"body_T_cam = Pose3(Rot3.Yaw(-np.pi / 2), Point3(0.1, 0, 0.2)) # Cam fwd = body +y\n",
"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",
"factor1 = PlanarProjectionFactor1(\n",
" X(0), landmark_pt, measured_pt2, body_T_cam, calib, measurement_noise\n",
")\n",
"factor1.print(\"Factor 1: \")\n",
"\n",
"# Evaluate error\n",
"values = Values()\n",
"values.insert(X(0), gt_pose2) # Error should be zero here\n",
"error1_gt = factor1.error(values)\n",
"print(f\"\\nError at ground truth: {error1_gt}\")\n",
"\n",
"noisy_pose2 = Pose2(1.05, 0.02, np.pi / 4 + 0.05)\n",
"values.update(X(0), 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 = { x0 l0 }\n",
"isotropic dim=2 sigma=1.5\n",
"\n",
"Error at ground truth: 0.0\n",
"Error with noisy landmark: 8066.192649473802\n"
]
}
],
"source": [
"factor2 = PlanarProjectionFactor2(\n",
" X(0), L(0), measured_pt2, body_T_cam, calib, measurement_noise\n",
")\n",
"factor2.print(\"Factor 2: \")\n",
"\n",
"# Evaluate error\n",
"values = Values()\n",
"values.insert(X(0), gt_pose2)\n",
"values.insert(L(0), landmark_pt) # Error should be zero\n",
"error2_gt = factor2.error(values)\n",
"print(f\"\\nError at ground truth: {error2_gt}\")\n",
"\n",
"noisy_landmark = Point3(2.1, 0.45, 0.55)\n",
"values.update(L(0), 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": 5,
"metadata": {
"id": "factor3_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Factor 3: keys = { x0 o0 c0 }\n",
"isotropic dim=2 sigma=1.5\n",
"\n",
"Error at ground truth: 0.0\n",
"Error with noisy calibration: 92.30212176019934\n"
]
}
],
"source": [
"offset_key = O(0)\n",
"calib_key = C(0)\n",
"\n",
"factor3 = PlanarProjectionFactor3(X(0), offset_key, calib_key, landmark_pt, measured_pt2, measurement_noise)\n",
"factor3.print(\"Factor 3: \")\n",
"\n",
"# Evaluate error\n",
"values = Values()\n",
"values.insert(X(0), gt_pose2)\n",
"values.insert(offset_key, body_T_cam)\n",
"values.insert(calib_key, calib) # Error should be zero\n",
"error3_gt = factor3.error(values)\n",
"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": "py312",
"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.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

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

@ -0,0 +1,202 @@
{
"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-develop"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Pose3, Rot3, Point3, Values, PoseRotationPrior3D\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": 2,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"PoseRotationPrior: PoseRotationPrior keys = { x0 }\n",
"isotropic dim=3 sigma=0.05\n",
"Measured Rotation [\n",
"\t0.707107, -0.707107, 0;\n",
"\t0.707107, 0.707107, 0;\n",
"\t0, 0, 1\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. PoseRotationPrior3D\n",
"factor = PoseRotationPrior3D(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 = PoseRotationPrior3D(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": 3,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Error with correct rotation: 0.0 (Should be near zero)\n",
"Error with incorrect rotation: 1.9999999999999951 (Should be non-zero)\n",
"Error with different translation: 1.9999999999999951 (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
}

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

@ -0,0 +1,204 @@
{
"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",
"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-develop"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import Pose3, Rot3, Point3, Values, PoseTranslationPrior3D\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": 4,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"PoseTranslationPrior: PoseTranslationPrior keys = { x0 }\n",
"isotropic dim=3 sigma=0.5\n",
"Measured Translation[\n",
"\t10;\n",
"\t20;\n",
"\t5\n",
"]\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 = PoseTranslationPrior3D(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 = PoseTranslationPrior3D(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": 5,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Error with correct translation: 0.0 (Should be near zero)\n",
"Error with incorrect translation: 0.11999999999999986 (Should be non-zero)\n",
"Error with different rotation: 0.11999999999999986 (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-develop"
]
},
{
"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
}

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

@ -0,0 +1,221 @@
{
"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-develop"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"['ReferenceFrameFactorPoint3Pose3']\n"
]
}
],
"source": [
"import gtsam\n",
"print([k for k in gtsam.__dict__.keys() if \"ReferenceFrame\" in k])"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"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.symbol_shorthand import L, T, O"
]
},
{
"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": 4,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"ReferenceFrameFactor: : ReferenceFrameFactor(Global: l0, Transform: t0, Local: o0)\n",
"isotropic dim=3 sigma=0.1\n"
]
}
],
"source": [
"global_landmark_key = L(0)\n",
"transform_key = T(0)\n",
"local_landmark_key = O(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": 5,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Expected local landmark: [ 2. -4. 1.]\n",
"\n",
"Error at ground truth: 4500.0 (Should be zero)\n",
"Error with noisy local landmark: 4621.125\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
}

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

@ -0,0 +1,225 @@
{
"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-develop"
]
},
{
"cell_type": "code",
"execution_count": 1,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"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": 2,
"metadata": {
"id": "factor1_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"RotateFactor: keys = { r0 }\n",
"isotropic dim=3 sigma=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: 700.0\n",
"Error at noisy R: 699.2869223608442\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": 8,
"metadata": {
"id": "factor2_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"RotateDirectionsFactor: keys = { r0 }\n",
"isotropic dim=2 sigma=0.01\n",
"RotateDirectionsFactor:\n",
"p:1\n",
"0\n",
"0\n",
"z:0\n",
"1\n",
"0\n",
"\n",
"Check: gt_R * z_body = \n",
": 1\n",
"6.12323e-17\n",
" 0\n",
"\n",
"Error at ground truth R: 1.874699728327322e-29\n",
"Error at noisy R: 0.999933335111092\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 = \\n{gt_R_dir.rotate(z_body)}\")\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
}

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

@ -0,0 +1,246 @@
{
"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.\n",
"\n",
"If you are using the factor, please cite:\n",
"> **L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert**, \"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors\", Int. Conf. on Robotics and Automation (ICRA), 2014."
]
},
{
"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": 1,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"try:\n",
" import google.colab\n",
" %pip install --quiet gtsam-develop\n",
"except ImportError:\n",
" pass # Not running on Colab, do nothing"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"import numpy as np\n",
"from gtsam import (\n",
" Values,\n",
" Point2,\n",
" Point3,\n",
" Pose3,\n",
" Rot3,\n",
" NonlinearFactorGraph,\n",
" SmartProjectionParams,\n",
" SmartProjectionFactorPinholeCameraCal3_S2,\n",
" PinholeCameraCal3_S2,\n",
" Cal3_S2,\n",
")\n",
"from gtsam.symbol_shorthand import C"
]
},
{
"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": 3,
"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 = 1\n",
"enableEPI = 0\n",
"landmarkDistanceThreshold = -1\n",
"dynamicOutlierRejectionThreshold = -1\n",
"useLOST = 0\n",
"noise model\n",
"\n",
"result:\n",
"no point, status = 1\n",
"\n",
"SmartFactorBase, z = \n",
"measurement 0, px = \n",
"150\n",
"505\n",
"noise model = unit (2) \n",
"measurement 1, px = \n",
"470\n",
"495\n",
"noise model = unit (2) \n",
"measurement 2, px = \n",
"480\n",
"150\n",
"noise model = unit (2) \n",
" keys = { 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": 4,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Triangulated point result:\n",
"Status.BEHIND_CAMERA\n",
"\n",
"Triangulation failed, error calculation depends on degeneracyMode.\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.status}\")\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.\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "py312",
"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.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

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

@ -0,0 +1,229 @@
{
"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": 2,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"try:\n",
" import google.colab\n",
" %pip install --quiet gtsam-develop\n",
"except ImportError:\n",
" pass # Not running on Colab, do nothing"
]
},
{
"cell_type": "code",
"execution_count": 3,
"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": 4,
"metadata": {
"id": "create_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Default Parameters:\n",
"linearizationMode: 0\n",
" degeneracyMode: 0\n",
"rankTolerance = 1\n",
"enableEPI = 0\n",
"landmarkDistanceThreshold = -1\n",
"dynamicOutlierRejectionThreshold = -1\n",
"useLOST = 0\n",
"noise model\n",
"\n",
"\n",
"Custom Parameters:\n",
"linearizationMode: 2\n",
" degeneracyMode: 1\n",
"rankTolerance = 1e-08\n",
"enableEPI = 0\n",
"landmarkDistanceThreshold = -1\n",
"dynamicOutlierRejectionThreshold = 1\n",
"useLOST = 0\n",
"noise model\n",
"\n"
]
}
],
"source": [
"# Default parameters\n",
"default_params = SmartProjectionParams()\n",
"print(\"Default Parameters:\")\n",
"default_params.print()\n",
"\n",
"# Custom parameters\n",
"custom_params = SmartProjectionParams(\n",
" linMode = LinearizationMode.JACOBIAN_Q,\n",
" degMode = DegeneracyMode.ZERO_ON_DEGENERACY,\n",
" throwCheirality = False,\n",
" verboseCheirality = True,\n",
" retriangulationTh = 1e-3\n",
")\n",
"# Modify triangulation parameters directly\n",
"custom_params.setRankTolerance(1e-8)\n",
"custom_params.setEnableEPI(False)\n",
"custom_params.setDynamicOutlierRejectionThreshold(3.0) # Reject points with reproj error > 3*sigma\n",
"\n",
"print(\"\\nCustom Parameters:\")\n",
"custom_params.print()"
]
},
{
"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": 5,
"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 = 0\n",
"landmarkDistanceThreshold = -1\n",
"dynamicOutlierRejectionThreshold = 1\n",
"useLOST = 0\n",
"noise model\n",
"\n",
"result:\n",
"no point, status = 1\n",
"\n",
"SmartFactorBase, z = \n",
" keys = { }\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 = 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": "py312",
"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.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

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

@ -0,0 +1,321 @@
{
"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`.\n",
"\n",
"If you are using the factor, please cite:\n",
"> **L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert**, \"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors\", Int. Conf. on Robotics and Automation (ICRA), 2014."
]
},
{
"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": 1,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"try:\n",
" import google.colab\n",
" %pip install --quiet gtsam-develop\n",
"except ImportError:\n",
" pass # Not running on Colab, do nothing"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"from gtsam import (\n",
" Values,\n",
" Point2,\n",
" Point3,\n",
" Pose3,\n",
" Rot3,\n",
" NonlinearFactorGraph,\n",
" SmartProjectionParams,\n",
" SmartProjectionPoseFactorCal3_S2,\n",
" Cal3_S2,\n",
")\n",
"from gtsam.symbol_shorthand import 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": 3,
"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 = 1\n",
"enableEPI = 0\n",
"landmarkDistanceThreshold = -1\n",
"dynamicOutlierRejectionThreshold = -1\n",
"useLOST = 0\n",
"noise model\n",
"\n",
"result:\n",
"no point, status = 1\n",
"\n",
"SmartFactorBase, z = \n",
"measurement 0, px = \n",
"150\n",
"505\n",
"noise model = unit (2) \n",
"measurement 1, px = \n",
"470\n",
"495\n",
"noise model = unit (2) \n",
"measurement 2, px = \n",
"480\n",
"150\n",
"noise model = unit (2) \n",
" keys = { x0 x1 x2 }\n"
]
}
],
"source": [
"smart_noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
"smart_params = SmartProjectionParams() # Use default params\n",
"K = Cal3_S2(500, 500, 0, 320, 240) # Fixed calibration\n",
"\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": 4,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Triangulated point result: Status.VALID\n",
"Triangulated point: [0.28416823 1.95555615 5.67688675]\n",
"\n",
"Total reprojection error (0.5 * sum(err^2/sigma^2)): 120243.1626\n"
]
}
],
"source": [
"# Create Values containing Pose3 objects\n",
"values = Values()\n",
"pose0 = Pose3(Rot3.Ypr(0.1, 0, 0), Point3(-1, 0, 0.5))\n",
"pose1 = Pose3(Rot3.Ypr(0.0, -0.1, 0), Point3(1, 0, 0.5))\n",
"pose2 = Pose3(Rot3.Ypr(0, 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: {point_result.status}\")\n",
"\n",
"if point_result.valid():\n",
" print(f\"Triangulated point: {point_result.get()}\")\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": 5,
"metadata": {
"id": "linearize_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Linearized Factor (showing HessianFactor structure):\n",
"\n",
" keys: x0(6) x1(6) x2(6) \n",
"Augmented information matrix: [\n",
"\t202102, 6373.47, -59741.9, 4386.91, -35145.2, 11091.1, -115337, -44197.1, 12233.6, -8181.39, 19268.4, -7521.18, -81683.2, -724.877, 5044.41, -484.005, 14404.6, -5767.48, 89097.4;\n",
"\t6373.47, 81115.2, -30292.7, 14717.5, -2404.93, -3343.21, 63410.4, 20584.6, -5335.01, 3787.66, -10602.2, 4111.71, -60743.7, -88128.4, 38992.5, -18242.3, 11072.3, -3393.47, 105458;\n",
"\t-59741.9, -30292.7, 27634, -6415.45, 10844.3, -1981.98, 10553.8, 5348.27, -1607.72, 998.024, -1760.06, 696.401, 44568.2, 31147.8, -15125.4, 6542.51, -7986, 2832.46, -62376.9;\n",
"\t4386.91, 14717.5, -6415.45, 2722.09, -996.496, -424.653, 9577.1, 3000.7, -765.229, 551.36, -1601.54, 620.327, -12253.7, -15890.6, 7106.5, -3294.65, 2225.84, -703.855, 20429.2;\n",
"\t-35145.2, -2404.93, 10844.3, -996.496, 6132.48, -1869.56, 18982.5, 7333.62, -2035.73, 1357.9, -3171.11, 1238.23, 15136.7, 1537.88, -1499.49, 376.242, -2675.09, 1054.42, -17138.8;\n",
"\t11091.1, -3343.21, -1981.98, -424.653, -1869.56, 777.22, -9389.67, -3428.6, 932.466, -633.633, 1569.05, -611.241, -1827.86, 3981.42, -1495.55, 805.333, 305.895, -169.934, 204.626;\n",
"\t-115337, 63410.4, 10553.8, 9577.1, 18982.5, -9389.67, 210063, -32012.2, 19847.7, -6615.86, -35357.8, 12992.1, -83063.6, 2877.07, 3675.5, 245.696, 14633.2, -5901.9, 91375.5;\n",
"\t-44197.1, 20584.6, 5348.27, 3000.7, 7333.62, -3428.6, -32012.2, 79820.6, -31086.7, 15340.5, 5564.44, -1509.55, 71381, -87978.3, 31244.3, -17668.1, -12223.3, 5946.04, -40235;\n",
"\t12233.6, -5335.01, -1607.72, -765.229, -2035.73, 932.466, 19847.7, -31086.7, 12383.1, -5991.81, -3406.72, 1051.42, -29836.9, 33051, -11561.9, 6625.01, 5124.61, -2447.34, 18485.3;\n",
"\t-8181.39, 3787.66, 998.024, 551.36, 1357.9, -633.633, -6615.86, 15340.5, -5991.81, 2949.34, 1147.27, -319.227, 13846.5, -16832.1, 5966.68, -3379.52, -2372.04, 1151.02, -7909.48;\n",
"\t19268.4, -10602.2, -1760.06, -1601.54, -3171.11, 1569.05, -35357.8, 5564.44, -3406.72, 1147.27, 5951.86, -2185.74, 14119.3, -690.008, -543.913, -82.7916, -2486.55, 1005.27, -15442.2;\n",
"\t-7521.18, 4111.71, 696.401, 620.327, 1238.23, -611.241, 12992.1, -1509.55, 1051.42, -319.227, -2185.74, 806.502, -4768.82, -371.498, 426.945, -95.4627, 842.323, -333.351, 5486.34;\n",
"\t-81683.2, -60743.7, 44568.2, -12253.7, 15136.7, -1827.86, -83063.6, 71381, -29836.9, 13846.5, 14119.3, -4768.82, 149690, -5708.64, -6412.92, -549.716, -26368.6, 10641.3, -162324;\n",
"\t-724.877, -88128.4, 31147.8, -15890.6, 1537.88, 3981.42, 2877.07, -87978.3, 33051, -16832.1, -690.008, -371.498, -5708.64, 160163, -64108.9, 32675.7, 347.645, -2041.12, -63443.5;\n",
"\t5044.41, 38992.5, -15125.4, 7106.5, -1499.49, -1495.55, 3675.5, 31244.3, -11561.9, 5966.68, -543.913, 426.945, -6412.92, -64108.9, 26167.1, -13114.9, 1394.39, 202.071, 34971.1;\n",
"\t-484.005, -18242.3, 6542.51, -3294.65, 376.242, 805.333, 245.696, -17668.1, 6625.01, -3379.52, -82.7916, -95.4627, -549.716, 32675.7, -13114.9, 6668.86, -37.4944, -372.943, -13620.5;\n",
"\t14404.6, 11072.3, -7986, 2225.84, -2675.09, 305.895, 14633.2, -12223.3, 5124.61, -2372.04, -2486.55, 842.323, -26368.6, 347.645, 1394.39, -37.4944, 4647.64, -1867.77, 28880.4;\n",
"\t-5767.48, -3393.47, 2832.46, -703.855, 1054.42, -169.934, -5901.9, 5946.04, -2447.34, 1151.02, 1005.27, -333.351, 10641.3, -2041.12, 202.071, -372.943, -1867.77, 773.191, -10827.4;\n",
"\t89097.4, 105458, -62376.9, 20429.2, -17138.8, 204.626, 91375.5, -40235, 18485.3, -7909.48, -15442.2, 5486.34, -162324, -63443.5, 34971.1, -13620.5, 28880.4, -10827.4, 240486\n",
"]\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.HessianFactor(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": "py312",
"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.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

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

@ -0,0 +1,586 @@
{
"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., `PinholePose`, `SphericalCamera`), provided they adhere to the expected GTSAM camera concepts. **Important Note:** See the **Note on Template Parameter `CAMERA`** below.\n",
"- **`Values` Requirement:** Requires `Pose3` objects (representing the body frame) in the `Values` container.\n",
"- **Configuration:** Behavior controlled by `SmartProjectionParams`. **Note:** Currently (as of C++ 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.\n",
"\n",
"**Note on Template Parameter `CAMERA`:**\n",
"While this factor is templated on `CAMERA` for generality, the current internal implementation for linearization has limitations. It implicitly assumes that `traits<CAMERA>::dimension` matches the optimized variable dimension (`Pose3::dimension`, which is 6).\n",
"Consequently:\n",
"- It works correctly with `CAMERA` types where `dimension == 6`, such as `PinholePose<CALIBRATION>` or `SphericalCamera`.\n",
"- Using `CAMERA` types where `dimension != 6`, such as `PinholeCamera<CALIBRATION>` (dim = 6 + CalDim), **will cause compilation errors**.\n",
"- **Recommendation:** For standard pinhole cameras in a fixed rig, **use `PinholePose<CALIBRATION>`** as the `CAMERA` type when defining the `CameraSet` for this factor.\n",
"\n",
"If you are using the factor, please cite:\n",
"> **L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert**, \"Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors\", Int. Conf. on Robotics and Automation (ICRA), 2014.\n"
]
},
{
"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": 1,
"metadata": {
"id": "pip_code",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"try:\n",
" import google.colab\n",
" %pip install --quiet gtsam-develop\n",
"except ImportError:\n",
" pass # Not running on Colab, do nothing"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "imports_code"
},
"outputs": [],
"source": [
"import gtsam\n",
"from gtsam import (\n",
" Values,\n",
" Point2,\n",
" Point3,\n",
" Pose3,\n",
" Rot3,\n",
" SmartProjectionParams,\n",
" LinearizationMode,\n",
" DegeneracyMode,\n",
" # Use PinholePose variant for wrapping\n",
" SmartProjectionRigFactorPinholePoseCal3_S2,\n",
" PinholePoseCal3_S2,\n",
" Cal3_S2,\n",
")\n",
"from gtsam.symbol_shorthand import X # Key for Pose3 variable (Body Pose)"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "create_header_md"
},
"source": [
"## Constructor"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "constructor_desc_md"
},
"source": [
"You create a `SmartProjectionRigFactor` by providing:\n",
"1. A noise model for the 2D pixel measurements (typically `noiseModel.Isotropic`).\n",
"2. A `CameraSet<CAMERA>` object defining the *fixed* rig configuration. Each `CAMERA` in the set contains its fixed intrinsics and its fixed pose relative to the rig's body frame (`body_P_camera`).\n",
"3. Optionally, `SmartProjectionParams` to configure linearization and degeneracy handling. Remember the current restrictions (HESSIAN, ZERO_ON_DEGENERACY)."
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "constructor_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"SmartFactorRig: SmartProjectionRigFactor: \n",
" SmartProjectionFactor\n",
"linearizationMode: 0\n",
"triangulationParameters:\n",
"rankTolerance = 1\n",
"enableEPI = 0\n",
"landmarkDistanceThreshold = -1\n",
"dynamicOutlierRejectionThreshold = -1\n",
"useLOST = 0\n",
"noise model\n",
"\n",
"result:\n",
"no point, status = 1\n",
"\n",
"SmartFactorBase, z = \n",
" keys = { }\n"
]
}
],
"source": [
"# Define the Camera Rig (using PinholePose)\n",
"K = Cal3_S2(500, 500, 0, 320, 240)\n",
"body_P_cam0 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, 0, 0))\n",
"cam0 = PinholePoseCal3_S2(body_P_cam0, K)\n",
"body_P_cam1 = Pose3(Rot3.Ypr(0, 0, 0), Point3(0.1, -0.1, 0)) # Baseline 0.1m\n",
"cam1 = PinholePoseCal3_S2(body_P_cam1, K)\n",
"\n",
"rig_cameras = gtsam.CameraSetPinholePoseCal3_S2()\n",
"rig_cameras.push_back(cam0)\n",
"rig_cameras.push_back(cam1)\n",
"\n",
"# Noise model and parameters\n",
"noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1.0)\n",
"smart_params = SmartProjectionParams(\n",
" linMode=LinearizationMode.HESSIAN, degMode=DegeneracyMode.ZERO_ON_DEGENERACY\n",
")\n",
"\n",
"# Create the Factor\n",
"smart_factor = SmartProjectionRigFactorPinholePoseCal3_S2(\n",
" noise_model, rig_cameras, smart_params\n",
")\n",
"smart_factor.print(\"SmartFactorRig: \")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "add_header_md"
},
"source": [
"## `add(measurement, poseKey, cameraId)`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "add_desc_md"
},
"source": [
"This method adds a single 2D measurement (`Point2`) associated with a specific camera in the rig and a specific body pose.\n",
"- `measurement`: The observed pixel coordinates.\n",
"- `poseKey`: The key (`Symbol` or `Key`) of the **body's `Pose3`** variable at the time of observation.\n",
"- `cameraId`: The integer index of the camera within the `CameraSet` (provided during construction) that captured this measurement."
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {
"id": "add_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Smart factor involves 2 measurements from 2 unique poses.\n"
]
}
],
"source": [
"# --- Use Pre-calculated Valid Measurements ---\n",
"# These measurements were calculated offline using:\n",
"# gt_landmark = Point3(1.0, 0.5, 5.0)\n",
"# gt_pose0 = Pose3()\n",
"# gt_pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0))\n",
"# And the rig defined above.\n",
"\n",
"z00 = Point2(400.0, 290.0) # Measurement from Body Pose X(0), Camera 0\n",
"z01 = Point2(350.0, 290.0) # Measurement from Body Pose X(0), Camera 1\n",
"z10 = Point2(372.787, 297.553) # Measurement from Body Pose X(1), Camera 0\n",
"z11 = Point2(323.308, 297.674) # Measurement from Body Pose X(1), Camera 1\n",
"# --------------------------------------------\n",
"\n",
"# 3. Add pre-calculated measurements\n",
"smart_factor.add(z00, X(0), 0)\n",
"smart_factor.add(z01, X(0), 1)\n",
"smart_factor.add(z10, X(1), 0)\n",
"smart_factor.add(z11, 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 (with pre-calculated measurements): \") # Optional\n"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "inherited_header_md"
},
"source": [
"## Inherited and Core Methods"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "error_header_md"
},
"source": [
"### `error(Values values)`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "error_desc_md"
},
"source": [
"Inherited from `SmartFactorBase`. Calculates the total reprojection error for the landmark.\n",
"**Internal Process:**\n",
"1. Retrieves the body `Pose3` estimates for all relevant keys from the `values` object.\n",
"2. For each measurement, calculates the corresponding camera's world pose using the body pose and the fixed rig extrinsics (`world_P_sensor = world_P_body * body_P_camera`).\n",
"3. Triangulates the 3D landmark position using these calculated camera poses and the stored 2D measurements.\n",
"4. Reprojects the triangulated point back into each calculated camera view.\n",
"5. Computes the sum of squared differences between the reprojections and the original measurements, weighted by the noise model.\n",
"6. Handles degenerate cases (e.g., failed triangulation) based on the `degeneracyMode` set in `SmartProjectionParams`."
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {
"id": "error_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Triangulation Result Status: Status.VALID\n",
"Total reprojection error (0.5 * sum(err^2/sigma^2)): 1316.4717\n"
]
}
],
"source": [
"# Assuming smart_factor created and measurements added above\n",
"\n",
"values = Values()\n",
"pose0 = Pose3() # Body at origin\n",
"pose1 = Pose3(Rot3.Ypr(0.1, 0.0, 0.0), Point3(0.5, 0, 0)) # Body moved\n",
"values.insert(X(0), pose0)\n",
"values.insert(X(1), pose1)\n",
"\n",
"# Need to check triangulation first, as error calculation depends on it\n",
"point_result = smart_factor.point(values)\n",
"print(f\"Triangulation Result Status: {point_result.status}\")\n",
"\n",
"total_error = smart_factor.error(values)\n",
"print(f\"Total reprojection error (0.5 * sum(err^2/sigma^2)): {total_error:.4f}\")\n",
"# Note: If degenerate and DegeneracyMode is ZERO_ON_DEGENERACY, error will be 0."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "point_header_md"
},
"source": [
"### `point(Values values)`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "point_desc_md"
},
"source": [
"Inherited from `SmartProjectionFactor`. Performs the internal triangulation based on the body poses in `values` and the fixed rig geometry.\n",
"Returns a `TriangulationResult` object which contains:\n",
"- The triangulated `Point3` (if successful).\n",
"- A status indicating whether the triangulation was `VALID`, `DEGENERATE`, `BEHIND_CAMERA`, `OUTLIER`, or `FAR_POINT`."
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {
"id": "point_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Triangulation Result Status: Status.VALID\n",
"Triangulated Point: [0.94370846 0.79793704 7.63497051]\n"
]
}
],
"source": [
"# Assuming smart_factor and values from the previous cell\n",
"point_result = smart_factor.point(values)\n",
"print(f\"Triangulation Result Status: {point_result.status}\")\n",
"\n",
"if point_result.valid():\n",
" triangulated_point = point_result.get()\n",
" print(f\"Triangulated Point: {triangulated_point}\")\n",
"else:\n",
" print(\"Triangulation did not produce a valid point.\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "cameras_header_md"
},
"source": [
"### `cameras(Values values)`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "cameras_desc_md"
},
"source": [
"Inherited from `SmartFactorBase`. Computes and returns a `CameraSet<CAMERA>` containing the effective cameras corresponding to *each measurement*.\n",
"For each measurement `i` associated with body pose key `k` and camera ID `cid`:\n",
"1. Retrieves the body pose `world_P_body = values.atPose3(k)`.\n",
"2. Retrieves the fixed relative pose `body_P_camera = rig_cameras.at(cid).pose()`.\n",
"3. Computes the camera's world pose `world_P_camera = world_P_body * body_P_camera`.\n",
"4. Creates a `CAMERA` object using this `world_P_camera` and the fixed intrinsics `rig_cameras.at(cid).calibration()`.\n",
"The returned `CameraSet` contains these calculated cameras, one for each measurement added via `add()`."
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {
"id": "cameras_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Pose of camera for measurement 0 (Body X(0), Cam 0):\n",
"R: [\n",
"\t1, 0, 0;\n",
"\t0, 1, 0;\n",
"\t0, 0, 1\n",
"]\n",
"t: 0.1 0 0\n",
"\n",
"\n",
"Pose of camera for measurement 1 (Body X(0), Cam 1):\n",
"R: [\n",
"\t1, 0, 0;\n",
"\t0, 1, 0;\n",
"\t0, 0, 1\n",
"]\n",
"t: 0.1 -0.1 0\n",
"\n",
"\n",
"Pose of camera for measurement 2 (Body X(1), Cam 0):\n",
"R: [\n",
"\t0.995004, -0.0998334, 0;\n",
"\t0.0998334, 0.995004, 0;\n",
"\t0, 0, 1\n",
"]\n",
"t: 0.5995 0.00998334 0\n",
"\n",
"\n",
"Pose of camera for measurement 3 (Body X(1), Cam 1):\n",
"R: [\n",
"\t0.995004, -0.0998334, 0;\n",
"\t0.0998334, 0.995004, 0;\n",
"\t0, 0, 1\n",
"]\n",
"t: 0.609484 -0.0895171 0\n",
"\n",
"\n"
]
}
],
"source": [
"# Assuming smart_factor and values from previous cells\n",
"\n",
"calculated_cameras = smart_factor.cameras(values)\n",
"\n",
"# Print the world pose of each calculated camera\n",
"print(f\"Pose of camera for measurement 0 (Body X(0), Cam 0):\\n{calculated_cameras.at(0).pose()}\\n\")\n",
"print(f\"Pose of camera for measurement 1 (Body X(0), Cam 1):\\n{calculated_cameras.at(1).pose()}\\n\")\n",
"print(f\"Pose of camera for measurement 2 (Body X(1), Cam 0):\\n{calculated_cameras.at(2).pose()}\\n\")\n",
"print(f\"Pose of camera for measurement 3 (Body X(1), Cam 1):\\n{calculated_cameras.at(3).pose()}\\n\")"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "linearize_header_md"
},
"source": [
"### `linearize(Values values)`"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "linearize_desc_md"
},
"source": [
"Inherited from `SmartProjectionFactor`. Computes the linear approximation (GaussianFactor) of the factor at the linearization point defined by `values`.\n",
"For `SmartProjectionRigFactor`, due to current implementation limitations, this **must** be configured via `SmartProjectionParams` to use `LinearizationMode.HESSIAN`.\n",
"The resulting `RegularHessianFactor` connects the **unique body pose keys** involved in the measurements."
]
},
{
"cell_type": "code",
"execution_count": 10,
"metadata": {
"id": "linearize_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"\n",
"Linearized Factor (HessianFactor structure):\n",
"Linear Factor: \n",
" keys: x0(6) x1(6) \n",
"Augmented information matrix: [\n",
"\t255621, 1454.13, -31747.6, 636.066, -33103.6, 3605.16, -254669, 22279.1, 15195.9, 2671.95, 33001.7, -3605.16, -5437.65;\n",
"\t1454.13, 9642.56, -1187.49, 1253.63, -198.336, -75.3949, -2405.75, -9411.71, 1088.32, -1227.56, 322.499, 75.3949, -653.552;\n",
"\t-31747.6, -1187.49, 4048.22, -209.638, 4112.44, -437.73, 31729.4, -1770.15, -1992, -201.969, -4112.82, 437.73, 740.416;\n",
"\t636.066, 1253.63, -209.638, 163.769, -83.6702, -3.45048, -757.87, -1182.15, 167.803, -154.598, 99.6018, 3.45048, -94.317;\n",
"\t-33103.6, -198.336, 4112.44, -83.6702, 4287, -466.758, 32981.3, -2875.28, -1968.94, -344.734, -4273.93, 466.758, 704.833;\n",
"\t3605.16, -75.3949, -437.73, -3.45048, -466.758, 51.9764, -3582.21, 409.075, 204.351, 50.0313, 464.082, -51.9764, -70.5256;\n",
"\t-254669, -2405.75, 31729.4, -757.87, 32981.3, -3582.21, 253816, -21248.6, -15238.8, -2538.55, -32892.2, 3582.21, 5479.25;\n",
"\t22279.1, -9411.71, -1770.15, -1182.15, -2875.28, 409.075, -21248.6, 11385.4, 332.508, 1463.29, "
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"2742.9, -409.075, 142.514;\n",
"\t15195.9, 1088.32, -1992, 167.803, -1968.94, 204.351, -15238.8, 332.508, 1007.53, 29.6019, 1975.86, -204.351, -387.999;\n",
"\t2671.95, -1227.56, -201.969, -154.598, -344.734, 50.0313, -2538.55, 1463.29, 29.6019, 188.241, 327.577, -50.0313, 23.48;\n",
"\t33001.7, 322.499, -4112.82, 99.6018, -4273.93, 464.082, -32892.2, 2742.9, 1975.86, 327.577, 4262.53, -464.082, -710.727;\n",
"\t-3605.16, 75.3949, 437.73, 3.45048, 466.758, -51.9764, 3582.21, -409.075, -204.351, -50.0313, -464.082, 51.9764, 70.5256;\n",
"\t-5437.65, -653.552, 740.416, -94.317, 704.833, -70.5256, 5479.25, 142.514, -387.999, 23.48, -710.727, 70.5256, 2632.94\n",
"]\n"
]
}
],
"source": [
"# Assuming smart_factor and values from previous cells\n",
"\n",
"# Check if triangulation succeeded before linearizing\n",
"if not smart_factor.point(values).valid():\n",
" print(\"Cannot linearize: triangulation failed or degenerate.\")\n",
"else:\n",
" linear_factor = smart_factor.linearize(values)\n",
"\n",
" if linear_factor:\n",
" print(\"\\nLinearized Factor (HessianFactor structure):\")\n",
" linear_factor.print(\"Linear Factor: \")\n",
" else:\n",
" print(\"\\nLinearization failed (potentially due to triangulation degeneracy and params setting).\")\n",
"\n",
"# Note: The printed Hessian is often zero when ZERO_ON_DEGENERACY is used and triangulation fails."
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "other_methods_header_md"
},
"source": [
"### Other Inherited Methods"
]
},
{
"cell_type": "markdown",
"metadata": {
"id": "other_methods_desc_md"
},
"source": [
"The factor also inherits standard methods from `NonlinearFactor` and `SmartFactorBase`:\n",
"- **`keys()`**: Returns a `KeyVector` containing the **unique body pose keys** involved.\n",
"- **`measured()`**: Returns a `Point2Vector` containing all the added 2D measurements.\n",
"- **`dim()`**: Returns the dimension of the error vector (2 * number of measurements).\n",
"- **`size()`**: Returns the number of measurements added.\n",
"- **`print(s, keyFormatter)`**: Prints details about the factor.\n",
"- **`equals(other, tol)`**: Compares two factors for equality."
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {
"id": "other_methods_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Keys: ['x0', 'x1']\n",
"Number of Measurements (size): 2\n",
"Dimension (dim): 8\n",
"Measurements: [array([400., 290.]), array([350., 290.]), array([372.787, 297.553]), array([323.308, 297.674])]\n"
]
}
],
"source": [
"# Assuming smart_factor from previous cells\n",
"print(f\"Keys: {[gtsam.DefaultKeyFormatter(k) for k in smart_factor.keys()]}\")\n",
"print(f\"Number of Measurements (size): {smart_factor.size()}\")\n",
"print(f\"Dimension (dim): {smart_factor.dim()}\")\n",
"print(f\"Measurements: {smart_factor.measured()}\")"
]
}
],
"metadata": {
"kernelspec": {
"display_name": "py312",
"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.12.6"
}
},
"nbformat": 4,
"nbformat_minor": 0
}

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

@ -0,0 +1,262 @@
{
"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",
"# Need StereoCamera for backprojection/triangulation\n",
"from gtsam import StereoCamera \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": 2,
"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": 3,
"metadata": {
"id": "eval_example_code"
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Expected landmark point (no offset): [ 1.54225239 -2.27112649 2.95849821]\n",
"\n",
"Error (no offset) at expected landmark: 48664.883462255115 (Should be near zero)\n",
"\n",
"Expected landmark point (offset): [ 2.89128008 -3.54882535 1.19789333]\n",
"Error (with offset) at recomputed landmark: 1783675.2295780657 (Should be near zero)\n",
"\n",
"Error (no offset) at noisy landmark: 54320.22670263611\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",
"values.insert(pose_key, pose)\n",
"\n",
"# --- Evaluate factor without offset --- \n",
"# Create a StereoCamera object at the current pose\n",
"camera_no_offset = StereoCamera(pose, K_stereo)\n",
"# Triangulate (backproject) the measurement to get the point in the camera frame\n",
"# Depth = fx * b / disparity = 500 * 0.1 / (330 - 305) = 50 / 25 = 2.0\n",
"expected_point_camera = camera_no_offset.backproject(measured_stereo) # Point in camera frame\n",
"# Transform the point from the camera frame to the world frame\n",
"landmark = pose.transformFrom(expected_point_camera) # Point in world frame\n",
"print(f\"Expected landmark point (no offset): {landmark}\")\n",
"\n",
"values.insert(landmark_key, landmark)\n",
"error_no_offset = factor_no_offset.error(values)\n",
"print(f\"\\nError (no offset) at expected landmark: {error_no_offset} (Should be near zero)\")\n",
"\n",
"# --- Evaluate factor with offset --- \n",
"# Calculate the actual sensor pose in the world\n",
"pose_with_offset = pose.compose(body_P_sensor) # world_P_sensor = world_P_body * body_P_sensor\n",
"# Create a StereoCamera object at the sensor pose\n",
"camera_with_offset = StereoCamera(pose_with_offset, K_stereo)\n",
"# Triangulate the measurement from the sensor's perspective\n",
"expected_point_offset_cam = camera_with_offset.backproject(measured_stereo) # Point in sensor frame\n",
"# Transform the point from the sensor frame to the world frame\n",
"landmark_offset = pose_with_offset.transformFrom(expected_point_offset_cam) # Point in world frame\n",
"print(f\"\\nExpected landmark point (offset): {landmark_offset}\")\n",
"\n",
"# Update the landmark value in Values for the offset factor calculation\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 near zero)\")\n",
"\n",
"# --- Evaluate with noisy landmark (using the no-offset factor for simplicity) ---\n",
"# Use the original landmark calculated for the no-offset case as the 'ground truth'\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\"\\nError (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-develop"
]
},
{
"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-develop"
]
},
{
"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
}

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

File diff suppressed because one or more lines are too long

203
gtsam/slam/slam.i
View File

@ -152,6 +152,30 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
void serialize() const;
};
// Following header defines PinholeCamera{Cal3_S2|Cal3DS2|Cal3Bundler|Cal3Fisheye|Cal3Unified}
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/slam/SmartFactorBase.h>
// Currently not wrapping SphericalCamera, since measurement type is not Point2 but Unit3
template <
CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2,
gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye,
gtsam::PinholeCameraCal3Unified, gtsam::PinholePoseCal3_S2,
gtsam::PinholePoseCal3DS2, gtsam::PinholePoseCal3Bundler,
gtsam::PinholePoseCal3Fisheye, gtsam::PinholePoseCal3Unified}>
virtual class SmartFactorBase : gtsam::NonlinearFactor {
void add(const gtsam::Point2& measured, size_t key);
void add(const gtsam::Point2Vector& measurements, const gtsam::KeyVector& cameraKeys);
size_t dim() const;
const std::vector<gtsam::Point2>& measured() const;
std::vector<CAMERA> cameras(const gtsam::Values& values) const;
void print(const std::string& s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::NonlinearFactor& p, double tol = 1e-9) const;
};
#include <gtsam/slam/SmartProjectionFactor.h>
/// Linearization mode: what factor to linearize to
@ -162,16 +186,86 @@ enum DegeneracyMode { IGNORE_DEGENERACY, ZERO_ON_DEGENERACY, HANDLE_INFINITY };
class SmartProjectionParams {
SmartProjectionParams();
SmartProjectionParams(gtsam::LinearizationMode linMode = gtsam::LinearizationMode::HESSIAN,
gtsam::DegeneracyMode degMode = gtsam::DegeneracyMode::IGNORE_DEGENERACY, bool throwCheirality = false,
bool verboseCheirality = false, double retriangulationTh = 1e-5);
void setLinearizationMode(gtsam::LinearizationMode linMode);
void setDegeneracyMode(gtsam::DegeneracyMode degMode);
void setRankTolerance(double rankTol);
void setEnableEPI(bool enableEPI);
void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold);
void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold);
void print(const std::string& str = "") const;
};
template <
CAMERA = {gtsam::PinholeCameraCal3_S2, gtsam::PinholeCameraCal3DS2,
gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3Fisheye,
gtsam::PinholeCameraCal3Unified, gtsam::PinholePoseCal3_S2,
gtsam::PinholePoseCal3DS2, gtsam::PinholePoseCal3Bundler,
gtsam::PinholePoseCal3Fisheye, gtsam::PinholePoseCal3Unified}>
virtual class SmartProjectionFactor : gtsam::SmartFactorBase<CAMERA> {
SmartProjectionFactor();
SmartProjectionFactor(
const gtsam::noiseModel::Base* sharedNoiseModel,
const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams());
bool decideIfTriangulate(const gtsam::CameraSet<CAMERA>& cameras) const;
gtsam::TriangulationResult triangulateSafe(const gtsam::CameraSet<CAMERA>& cameras) const;
bool triangulateForLinearize(const gtsam::CameraSet<CAMERA>& cameras) const;
gtsam::HessianFactor createHessianFactor(
const gtsam::CameraSet<CAMERA>& cameras, const double lambda = 0.0,
bool diagonalDamping = false) const;
gtsam::JacobianFactor createJacobianQFactor(
const gtsam::CameraSet<CAMERA>& cameras, double lambda) const;
gtsam::JacobianFactor createJacobianQFactor(
const gtsam::Values& values, double lambda) const;
gtsam::JacobianFactor createJacobianSVDFactor(
const gtsam::CameraSet<CAMERA>& cameras, double lambda) const;
gtsam::HessianFactor linearizeToHessian(
const gtsam::Values& values, double lambda = 0.0) const;
gtsam::JacobianFactor linearizeToJacobian(
const gtsam::Values& values, double lambda = 0.0) const;
gtsam::GaussianFactor linearizeDamped(const gtsam::CameraSet<CAMERA>& cameras,
const double lambda = 0.0) const;
gtsam::GaussianFactor linearizeDamped(const gtsam::Values& values,
const double lambda = 0.0) const;
gtsam::GaussianFactor linearize(
const gtsam::Values& values) const;
bool triangulateAndComputeE(gtsam::Matrix& E, const gtsam::CameraSet<CAMERA>& cameras) const;
bool triangulateAndComputeE(gtsam::Matrix& E, const gtsam::Values& values) const;
gtsam::Vector reprojectionErrorAfterTriangulation(const gtsam::Values& values) const;
double totalReprojectionError(const gtsam::CameraSet<CAMERA>& cameras,
gtsam::Point3 externalPoint) const;
double error(const gtsam::Values& values) const;
gtsam::TriangulationResult point() const;
gtsam::TriangulationResult point(const gtsam::Values& values) const;
bool isValid() const;
bool isDegenerate() const;
bool isPointBehindCamera() const;
bool isOutlier() const;
bool isFarPoint() const;
};
#include <gtsam/slam/SmartProjectionPoseFactor.h>
template <CALIBRATION>
// We are not deriving from SmartProjectionFactor yet - too complicated in wrapper
template <CALIBRATION = {gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler,
gtsam::Cal3Fisheye, gtsam::Cal3Unified}>
virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K);
@ -195,8 +289,30 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
gtsam::TriangulationResult point(const gtsam::Values& values) const;
};
typedef gtsam::SmartProjectionPoseFactor<gtsam::Cal3_S2>
SmartProjectionPose3Factor;
#include <gtsam/slam/SmartProjectionRigFactor.h>
// Only for PinholePose cameras -> PinholeCamera is not supported
template <CAMERA = {gtsam::PinholePoseCal3_S2, gtsam::PinholePoseCal3DS2,
gtsam::PinholePoseCal3Bundler,
gtsam::PinholePoseCal3Fisheye,
gtsam::PinholePoseCal3Unified}>
virtual class SmartProjectionRigFactor : gtsam::SmartProjectionFactor<CAMERA> {
SmartProjectionRigFactor();
SmartProjectionRigFactor(
const gtsam::noiseModel::Base* sharedNoiseModel,
const gtsam::CameraSet<CAMERA>* cameraRig,
const gtsam::SmartProjectionParams& params = gtsam::SmartProjectionParams());
void add(const gtsam::Point2& measured, const gtsam::Key& poseKey,
const size_t& cameraId = 0);
void add(const gtsam::Point2Vector& measurements, const gtsam::KeyVector& poseKeys,
const gtsam::FastVector<size_t>& cameraIds = gtsam::FastVector<size_t>());
const gtsam::KeyVector& nonUniqueKeys() const;
const gtsam::CameraSet<CAMERA>& cameraRig() const;
const gtsam::FastVector<size_t>& cameraIds() const;
};
#include <gtsam/slam/StereoFactor.h>
template <POSE, LANDMARK>
@ -227,11 +343,72 @@ virtual class GenericStereoFactor : gtsam::NoiseModelFactor {
typedef gtsam::GenericStereoFactor<gtsam::Pose3, gtsam::Point3>
GenericStereoFactor3D;
#include <gtsam/slam/ReferenceFrameFactor.h>
template<LANDMARK = {gtsam::Point3}, POSE = {gtsam::Pose3}>
class ReferenceFrameFactor : gtsam::NoiseModelFactor {
ReferenceFrameFactor(gtsam::Key globalKey, gtsam::Key transKey,
gtsam::Key localKey, const gtsam::noiseModel::Base* model);
gtsam::Vector evaluateError(const LANDMARK& global, const POSE& trans, const LANDMARK& local);
void print(const std::string& s="",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter);
};
#include <gtsam/slam/RotateFactor.h>
class RotateFactor : gtsam::NoiseModelFactor {
RotateFactor(gtsam::Key key, const gtsam::Rot3& P, const gtsam::Rot3& Z,
const gtsam::noiseModel::Base* model);
void print(const std::string& s = "",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
gtsam::Vector evaluateError(const gtsam::Rot3& R) const;
};
class RotateDirectionsFactor : gtsam::NoiseModelFactor {
RotateDirectionsFactor(gtsam::Key key, const gtsam::Unit3& i_p, const gtsam::Unit3& c_z,
const gtsam::noiseModel::Base* model);
static gtsam::Rot3 Initialize(const gtsam::Unit3& i_p, const gtsam::Unit3& c_z);
void print(const std::string& s = "",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
gtsam::Vector evaluateError(const gtsam::Rot3& iRc) const;
};
#include <gtsam/slam/OrientedPlane3Factor.h>
class OrientedPlane3Factor : gtsam::NoiseModelFactor {
OrientedPlane3Factor();
OrientedPlane3Factor(const gtsam::Vector& z, const gtsam::noiseModel::Gaussian* noiseModel,
gtsam::Key poseKey, gtsam::Key landmarkKey);
void print(const std::string& s = "OrientedPlane3Factor",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
gtsam::Vector evaluateError(
const gtsam::Pose3& pose, const gtsam::OrientedPlane3& plane) const;
};
class OrientedPlane3DirectionPrior : gtsam::NoiseModelFactor {
OrientedPlane3DirectionPrior();
OrientedPlane3DirectionPrior(gtsam::Key key, const gtsam::Vector& z,
const gtsam::noiseModel::Gaussian* noiseModel);
void print(const std::string& s = "OrientedPlane3DirectionPrior",
const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) const;
gtsam::Vector evaluateError(const gtsam::OrientedPlane3& plane) const;
};
#include <gtsam/slam/PoseTranslationPrior.h>
template <POSE>
virtual class PoseTranslationPrior : gtsam::NoiseModelFactor {
PoseTranslationPrior(size_t key, const POSE::Translation& measured,
const gtsam::noiseModel::Base* model);
PoseTranslationPrior(size_t key, const POSE& pose_z,
const gtsam::noiseModel::Base* noiseModel);
const gtsam::noiseModel::Base* model);
POSE::Translation measured() const;
// enabling serialization functionality
@ -244,8 +421,10 @@ typedef gtsam::PoseTranslationPrior<gtsam::Pose3> PoseTranslationPrior3D;
#include <gtsam/slam/PoseRotationPrior.h>
template <POSE>
virtual class PoseRotationPrior : gtsam::NoiseModelFactor {
PoseRotationPrior(size_t key, const POSE::Rotation& rot_z,
const gtsam::noiseModel::Base* model);
PoseRotationPrior(size_t key, const POSE& pose_z,
const gtsam::noiseModel::Base* noiseModel);
const gtsam::noiseModel::Base* model);
POSE::Rotation measured() const;
};
@ -396,14 +575,25 @@ template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
virtual class KarcherMeanFactor : gtsam::NonlinearFactor {
KarcherMeanFactor(const gtsam::KeyVector& keys);
KarcherMeanFactor(const gtsam::KeyVector& keys, int d, double beta);
};
gtsam::Rot3 FindKarcherMean(const gtsam::Rot3Vector& rotations);
template <T = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose3}>
T FindKarcherMean(const std::vector<T>& elements);
#include <gtsam/slam/FrobeniusFactor.h>
gtsam::noiseModel::Isotropic* ConvertNoiseModel(gtsam::noiseModel::Base* model,
size_t d);
template <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3}>
class FrobeniusPrior : gtsam::NoiseModelFactor {
FrobeniusPrior(gtsam::Key j, const gtsam::Matrix& M,
const gtsam::noiseModel::Base* model);
gtsam::Vector evaluateError(const T& g) const;
};
template <T = {gtsam::Rot2, gtsam::Rot3, gtsam::SO3, gtsam::SO4, gtsam::Pose2, gtsam::Pose3}>
virtual class FrobeniusFactor : gtsam::NoiseModelFactor {
FrobeniusFactor(size_t key1, size_t key2);
@ -464,6 +654,7 @@ typedef gtsam::TriangulationFactor<gtsam::PinholePose<gtsam::Cal3Unified>>
namespace lago {
gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true);
gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialGuess);
gtsam::VectorValues initializeOrientations(const gtsam::NonlinearFactorGraph& graph, bool useOdometricPath = true);
}
} // namespace gtsam

54
gtsam/slam/slam.md Normal file
View File

@ -0,0 +1,54 @@
# SLAM
The `slam` module provides a collection of factors, constraints, utilities, and initialization algorithms commonly used in Simultaneous Localization and Mapping (SLAM) and Structure from Motion (SfM) applications. It builds upon the core GTSAM inference engine (`gtsam/inference`) and geometric types (`gtsam/geometry`).
## Core Factors
These are fundamental factor types often used as building blocks in SLAM.
- [PriorFactor](doc/PriorFactor.ipynb) : A prior factor acting only on the rotation component of a pose variable.
- [BetweenFactor](doc/BetweenFactor.ipynb) : Represents relative measurements between two poses or other Lie group variables (e.g., derived from [odometry](https://en.wikipedia.org/wiki/Odometry)).
## Visual SLAM/SfM Factors
Factors specifically designed for visual data (camera measurements).
- [GenericProjectionFactor](doc/GenericProjectionFactor.ipynb) : Standard monocular projection factor relating a 3D landmark, camera pose, and fixed calibration to a 2D measurement.
- [GeneralSFMFactor](doc/GeneralSFMFactor.ipynb) : Projection factors used when camera calibration is unknown or optimized alongside poses and landmarks.
- [StereoFactor](doc/StereoFactor.ipynb) : Standard stereo projection factor relating a 3D landmark, camera pose, and fixed stereo calibration to a `StereoPoint2` measurement.
- [EssentialMatrixFactor](doc/EssentialMatrixFactor.ipynb) : Factors constraining poses or calibration based on the Essential matrix derived from calibrated cameras.
- [EssentialMatrixConstraint](doc/EssentialMatrixConstraint.ipynb) : Factor constraining the relative pose between two cameras based on a measured Essential matrix.
- [TriangulationFactor](doc/TriangulationFactor.ipynb) : Factor constraining a 3D point based on a measurement from a single known camera view, useful for triangulation.
- [PlanarProjectionFactor](doc/PlanarProjectionFactor.ipynb) : Projection factors specialized for robots moving on a 2D plane.
## Smart Factors
Factors that implicitly manage landmark variables, marginalizing them out during optimization.
- [SmartFactorParams](doc/SmartFactorParams.ipynb) : Configuration parameters controlling the behavior of smart factors (linearization, degeneracy handling, etc.).
- [SmartProjectionFactor](doc/SmartProjectionFactor.ipynb) : Smart factor for monocular measurements where both camera pose and calibration are optimized.
- [SmartProjectionPoseFactor](doc/SmartProjectionPoseFactor.ipynb) : Smart factor for monocular measurements where camera calibration is fixed, optimizing only poses.
- [SmartProjectionRigFactor](doc/SmartProjectionRigFactor.ipynb) : Smart factor for calibrated multi-camera rigs, optimizing only the rig's body pose.
- [SmartFactorBase](https://github.com/borglab/gtsam/blob/develop/gtsam/slam/SmartFactorBase.h) : Abstract base class for smart factors (internal use).
## Other Geometric Factors & Constraints
Factors representing various geometric relationships or constraints.
- [PoseRotationPrior](doc/PoseRotationPrior.ipynb) : A prior factor acting only on the rotation component of a pose variable.
- [PoseTranslationPrior](doc/PoseTranslationPrior.ipynb) : A prior factor acting only on the translation component of a pose variable.
- [OrientedPlane3Factor](doc/OrientedPlane3Factor.ipynb) : Factors for estimating and constraining 3D planar landmarks (`OrientedPlane3`).
- [RotateFactor](doc/RotateFactor.ipynb) : Factors constraining an unknown rotation based on how it transforms measured rotations or directions.
- [KarcherMeanFactor](doc/KarcherMeanFactor.ipynb) : Factor for constraining the Karcher mean (geometric average) of a set of rotations or other manifold values.
- [FrobeniusFactor](doc/FrobeniusFactor.ipynb) : Factors operating directly on rotation matrix entries using the Frobenius norm, an alternative to Lie algebra-based factors.
- [ReferenceFrameFactor](doc/ReferenceFrameFactor.ipynb) : Factor relating the same landmark observed in two different coordinate frames via an unknown transformation, useful for map merging.
- [BoundingConstraint](doc/BoundingConstraint.ipynb) : Abstract base class for creating inequality constraints (e.g., keeping a variable within certain bounds). Requires C++ derivation.
- [AntiFactor](doc/AntiFactor.ipynb) : A factor designed to negate the effect of another factor, useful for dynamically removing constraints.
## Initialization & Utilities
Helper functions and classes for SLAM tasks.
- [lago](doc/lago.ipynb) : Linear Approximation for Graph Optimization (LAGO) for initializing `Pose2` graphs.
- [InitializePose3](doc/InitializePose3.ipynb) : Methods for initializing `Pose3` graphs by first solving for rotations, then translations.
- [dataset](doc/dataset.ipynb) : Utility functions for loading/saving common SLAM dataset formats (g2o, TORO).
- [expressions](https://github.com/borglab/gtsam/blob/develop/gtsam/slam/expressions.h) : Pre-defined Expression trees for common SLAM factor types (internal use for Expression-based factors).

4
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
View File

@ -38,10 +38,10 @@ for i=0:length(measurements)
projectionFactorSeenBy = [];
if options.includeCameraFactors == 1
for j=1:options.numberOfLandmarks
SmartProjectionFactors(j) = SmartProjectionPose3Factor(0.01);
SmartProjectionFactors(j) = SmartProjectionPoseFactorCal3_S2(0.01);
% Use constructor with default values, but express the pose of the
% camera as a 90 degree rotation about the X axis
% SmartProjectionFactors(j) = SmartProjectionPose3Factor( ...
% SmartProjectionFactors(j) = SmartProjectionPoseFactorCal3_S2( ...
% 1, ... % rankTol
% -1, ... % linThreshold
% false, ... % manageDegeneracy

3
myst.yml
View File

@ -25,6 +25,9 @@ project:
- file: ./gtsam/navigation/navigation.md
children:
- pattern: ./gtsam/navigation/doc/*
- file: ./gtsam/slam/slam.md
children:
- pattern: ./gtsam/slam/doc/*
- file: ./doc/examples.md
children:
- pattern: ./python/gtsam/examples/*.ipynb

6
python/gtsam/tests/test_KarcherMeanFactor.py
View File

@ -35,7 +35,7 @@ class TestKarcherMean(GtsamTestCase):
"""
rotations = [R, R.inverse()]
expected = Rot3()
actual = gtsam.FindKarcherMean(rotations)
actual = gtsam.FindKarcherMeanRot3(rotations)
self.gtsamAssertEquals(expected, actual)
def test_find_karcher_mean_identity(self):
@ -47,7 +47,7 @@ class TestKarcherMean(GtsamTestCase):
aRb_list = [a1Rb1, a2Rb2, a3Rb3]
aRb_expected = Rot3()
aRb = gtsam.FindKarcherMean(aRb_list)
aRb = gtsam.FindKarcherMeanRot3(aRb_list)
self.gtsamAssertEquals(aRb, aRb_expected)
def test_factor(self):
@ -69,7 +69,7 @@ class TestKarcherMean(GtsamTestCase):
expected = Rot3()
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
actual = gtsam.FindKarcherMean([result.atRot3(1), result.atRot3(2)])
actual = gtsam.FindKarcherMeanRot3([result.atRot3(1), result.atRot3(2)])
self.gtsamAssertEquals(expected, actual)
self.gtsamAssertEquals(R12, result.atRot3(1).between(result.atRot3(2)))

6
python/gtsam/tests/test_backwards_compatibility.py
View File

@ -315,7 +315,7 @@ class TestBackwardsCompatibility(GtsamTestCase):
rotations = gtsam.Rot3Vector([R, R.inverse()])
expected = Rot3()
actual = gtsam.FindKarcherMean(rotations)
actual = gtsam.FindKarcherMeanRot3(rotations)
self.gtsamAssertEquals(expected, actual)
def test_find_karcher_mean_identity(self):
@ -327,7 +327,7 @@ class TestBackwardsCompatibility(GtsamTestCase):
aRb_list = gtsam.Rot3Vector([a1Rb1, a2Rb2, a3Rb3])
aRb_expected = Rot3()
aRb = gtsam.FindKarcherMean(aRb_list)
aRb = gtsam.FindKarcherMeanRot3(aRb_list)
self.gtsamAssertEquals(aRb, aRb_expected)
def test_factor(self):
@ -354,7 +354,7 @@ class TestBackwardsCompatibility(GtsamTestCase):
expected = Rot3()
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
actual = gtsam.FindKarcherMean(
actual = gtsam.FindKarcherMeanRot3(
gtsam.Rot3Vector([result.atRot3(1),
result.atRot3(2)]))
self.gtsamAssertEquals(expected, actual)

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

@ -13,7 +13,7 @@ import unittest
import numpy as np
import gtsam
from gtsam import Point3, Pose2, PriorFactorPose2, Values
from gtsam import BetweenFactorPose2, Point3, Pose2, PriorFactorPose2, Values
class TestLago(unittest.TestCase):
@ -33,6 +33,32 @@ class TestLago(unittest.TestCase):
estimateLago: Values = gtsam.lago.initialize(graph)
assert isinstance(estimateLago, Values)
def test_initialize2(self) -> None:
"""Smokescreen to ensure LAGO can be imported and run on toy data stored in a g2o file."""
# 1. Create a NonlinearFactorGraph with Pose2 factors
graph = gtsam.NonlinearFactorGraph()
# Add a prior on the first pose
prior_mean = Pose2(0.0, 0.0, 0.0)
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.05]))
graph.add(PriorFactorPose2(0, prior_mean, prior_noise))
# Add odometry factors (simulating moving in a square)
odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
graph.add(BetweenFactorPose2(0, 1, Pose2(2.0, 0.0, 0.0), odometry_noise))
graph.add(BetweenFactorPose2(1, 2, Pose2(2.0, 0.0, np.pi / 2), odometry_noise))
graph.add(BetweenFactorPose2(2, 3, Pose2(2.0, 0.0, np.pi / 2), odometry_noise))
graph.add(BetweenFactorPose2(3, 4, Pose2(2.0, 0.0, np.pi / 2), odometry_noise))
# Add a loop closure factor
loop_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25, 0.15]))
# Ideal loop closure would be Pose2(2.0, 0.0, np.pi/2)
measured_loop = Pose2(2.1, 0.1, np.pi / 2 + 0.05)
graph.add(BetweenFactorPose2(4, 0, measured_loop, loop_noise))
estimateLago: Values = gtsam.lago.initialize(graph)
assert isinstance(estimateLago, Values)
if __name__ == "__main__":
unittest.main()